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ABSTRACT 


This  thesis  describes  a  method  of  computing  a  feasible  path  solution  for 
the  anisotropic  weighted  region  problem.  Heuristics  are  used  to  locate  an 
initial  starting  solution.  This  starting  solution  is  iteratively  improved  using  a 
golden  ratio  search  to  produce  a  solution  within  a  specified  tolerance.  The 
path  solution  is  then  randomly  perturbed  or  detoured  through  different 
region  frontiers,  and  the  golden  ratio  search  is  again  applied.  These  random 
detours  are  controlled  by  a  process  known  as  simulated  annealing,  which 
determines  the  number  of  detours  made  and  decides  whether  to  accept  or 
reject  each  path  solution.  Better  solutions  are  always  accepted  and  worse 
solutions  are  accepted  based  on  a  probability  distribution.  Accepting  worse 
solutions  allows  an  opportunity  to  escape  from  a  local  minimum  condition 
and  continue  the  search  for  the  optimal  path.  Since  an  exhaustive  search  is 
not  performed,  the  globally  optimal  path  may  not  be  found,  but  a  feasible  path 
can  be  found  with  this  method. 
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I.  INTRODUCTION 


A.  GENERAL  BACKGROUND 

Military  applications  of  autonomous  vehicles  are  twofold:  to  delegate 
certain  repetitive  tasks  to  machines,  allowing  more  efficient  use  of  scarce 
human  resources,  and  to  avoid  exposing  humans  to  hazardous  duties  that 
could  be  satisfactorily  performed  by  automated  machines.  An  autonomous 
vehicle  could  transport  supplies  from  a  rear  area  support  facility  to  a  front 
line  unit.  It  could  also  perform  surveillance  and  measurement  functions  in 
areas  contaminated  by  chemical  or  radiological  agents.  To  accomplish  these 
tasks,  a  vehicle  must  be  capable  of  examining  terrain  data  and  selecting  a  path 
which  permits  efficient  travel  from  a  start  point  to  a  given  destination. 

This  process  begins  with  high-level  path  planning,  where  the  vehicle 
may  take  into  account  terrain,  weather,  concealment,  and  exposure  to  hostile 
forces  in  selecting  the  general  route  to  take.  One  way  to  model  this  task  is  by 
using  the  weighted  region  problem. 

B.  THE  WEIGHTED  REGION  PROBLEM 

1.  Definition 

The  weighted  region  problem  accepts  input  from  a  two-dimensional 
Cartesian  map  which  models  terrain  as  convex  polygonal  regions.  Each 
region  is  assigned  a  cost  coefficient  or  weight  which  is  the  cost  per  unit 
distance  travelled,  relative  to  other  regions  in  the  map.  The  cost  of  traversing 
a  region  is  calculated  by  multiplying  the  region's  weight  by  the  Euclidean 
distance  travelled  through  the  region.  See  Figure  1. 
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Figure  1.  Weighted  Region  Problem 


Given  a  start  point  and  destination  point  in  terms  of  map 
coordinates,  the  goal  is  to  find  the  most  efficient  path  from  start  to  destination 
through  the  weighted  regions. 
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An  extension  of  the  weighted  region  problem  involves  finding  a  path 
through  sloped  regions,  where  a  ground  vehicle's  direction  of  travel  is 
limited  by  its  ability  to  climb  steep  hills  and  travel  across  sideslopes. 

Researchers  describe  this  path  planning  problem  as  the  anisotropic 
weighted  region  problem.  Anisotropic  means  that  the  direction  of  travel  also 
influences  the  cost  of  traversing  a  region.  For  example,  it  requires  more 
energy  to  travel  uphill  on  a  path  than  it  does  to  travel  downhill  on  the  same 
path.  A  heading  which  would  cause  a  vehicle  to  exceed  its  center-of-gravity 
limits  can  be  avoided  by  assigning  an  infinite  (or  sufficiently  large)  cost  to  this 
path.  Because  of  the  added  dimension  of  elevation,  this  problem  is 
sometimes  referred  to  as  2\  dimensional  path  planning. 

2.  Problems  to  Overcome 

An  efficient  path  planning  program  is  not  trivial.  This  program 
must  select  each  segment  of  the  path  by  comparing  the  surface  condition  and 
slope  of  each  region  with  the  physical  limitations  of  the  vehicle.  A  method  of 
partitioning  the  terrain  must  be  determined.  One  way  is  to  divide  the  terrain 
into  a  grid  such  that  each  grid  square  possesses  homogeneous  terrain  with 
respect  to  surface  composition,  slope,  and  traversal  cost.  While  this  method 
may  be  relatively  simple  to  implement,  a  large  number  of  grid  squares 
requires  large  memory  allocation  and  the  solution  path  may  not  be  safe  due 
to  the  "stair  stepping"  characteristics  of  moving  from  one  grid  square  to  a 
neighboring  grid  square. 

In  another  technique,  partitions  are  only  made  when  necessary.  A 
region  is  defined  by  homogeneous  terrain,  and  is  independent  of  size  or 
shape.  This  approach  reduces  memory  requirements,  but  complicates  the 
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sea’-ch  for  the  optimal  path  because  the  traditional  "shortest  path"  graph 
search  algorithms  do  not  provide  sufficient  resolution  to  obtain  an  efficient 
path.  Most  implementations  of  this  technique  restrict  the  regions  to  convex 
polygons  which  simplifies  the  search  and  cost-calculating  processes. 

The  vehicle  must  avoid  areas  where  its  power  or  stability  limitations 
are  exceeded.  If  not,  the  vehicle  will  find  itself  facing  a  hill  which  it  cannot 
climb  or  a  sideslope  which  will  cause  it  to  overturn.  Paths  which  lead 
downhill  may  also  be  undesirable.  If  the  destination's  elevation  is  equal  to  or 
higher  than  the  start  point's  elevation,  then  travelling  downhill  only 
increases  the  net  elevation  the  vehicle  must  climb  in  order  to  reach  its 
destination. 

Once  the  program  eliminates  non-negotiable  terrain  regions  from 
consideration,  it  must  examine  the  remaining  regions  and  select  an  acceptable 
path.  These  remaining  regions  may  or  may  not  be  traversable,  depending 
upon  direction  of  travel.  Again,  the  vehicle  must  avoid  path  headings  that 
lead  to  excessively  steep  climbs  or  a  potential  rollover  condition. 

The  calculations  required  to  conduct  an  exhaustive  search  for  an 
optimal  path  in  2^  dimensional  space  grow  exponentially  with  the  number  of 
regions  in  the  sciirch  space.  Accuracy  is  a  function  of  resolution  of  the  search 
space.  Greater  accuracy  requires  smaller  grids  or  regions  that  provide  a  more 
accurate  representation  of  the  terrain,  but  increase  the  number  of  calculations 
necessary  to  investigate  each  segment  of  the  search  space  (or  map).  This 
computational  workload  requires  the  use  of  a  computer  to  speed  up  the 
process  and  determine  the  results  in  a  reasonable  amount  of  time. 
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Even  with  the  aid  of  a  computer,  the  time  required  to  determine  an 
optimal  path  may  be  excessive.  Current  algorithms  do  not  find  an  optimal 
path  through  2\  dimensional  space  within  acceptable  time  limits. 
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II.  PREVIOUS  WORK 


A.  ALGORITHMS 

Several  algorithms  have  been  developed  to  solve  the  weighted  region 
problem,  including  wavefront  propagation  [Ref.  1]  and  the  continuous 
Dijkstra  algorithm  [Ref.  2].  The  algorithms  most  impacting  on  this  work  are 
the  systematic  search  by  Ross  and  the  stochastic  approach  by  Kindi. 

B.  SYSTEMATIC  SEARCH 

Ross  [Ref.  3]  describes  an  algorithm  which  finds  an  optimal  path  through 
anisotropic  weighted  regions.  He  implemented  his  work  with  the  Common 
Lisp  programming  language.  Ross’s  algorithm  used  a  map  that  was  divided 
into  homogeneous  regions.  The  area  within  a  region  shares  a  common  slope, 
orientation,  surface  composition  and  condition,  stability  and  braking 
constraints. 

He  also  defined  a  vehicle  model  which  represents  slop)e,  climb,  stability, 
and  braking  limits.  He  extensively  described  the  trigonometric  and  physics 
equations  used  to  determine  which  headings  were  allowable  in  each  region. 

Ross  used  an  A*  search  algorithm  to  find  feasible  paths  to  investigate.  He 
calls  a  sequence  of  regions  that  contains  a  feasible  path  from  start  to  goal  a 
window  sequence.  See  Figure  2.  For  each  feasible  window  sequence,  the 
algorithm  conducts  an  exhaustive  search  of  all  paths  that  lie  within  the 
permissible  heading  ranges. 

Although  this  procedure  finds  the  globally  optimal  path,  the  time 
required  to  find  it  renders  the  program  impractical.  Test  results  show  it  may 
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require  as  much  as  29  minutes  to  calculate  a  globally  optimal  path  on  a  simple 
map  containing  40  regions. 


C  STCXMASTIC  APPROACH 

Kindi  [Ref.  4]  introduced  a  stochastic  approach  to  solving  the  isotropic 
weighted  region  problem.  He  implemented  his  work  using  the  C++  and 
Prolog  languages.  His  approach  is  based  on  a  technique  called  simulated 
annealing.  Like  Ross,  Kindi  defined  the  regions  on  a  map  by  edges  and 
vertices.  Kindi  placed  a  node  at  the  mid-point  of  each  edge  and  defined  a 
series  of  arcs  which  connect  every  node  in  a  region.  He  calls  this  structure  an 
edge  dual  graph.  See  Figure  3.  Kindi  then  Lnds  a  feasible  window  sequence 
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by  performing  an  A*  search  on  the  edge  dual  graph.  This  path  is  the  basis  for 
finding  a  locally  optimal  path  through  the  generated  window  sequence.  The 
locally  optimal  path  is  found  by  iteratively  applying  golden  ratio  search  to  the 
path  segments. 


Figiu-e  3.  Edge  Dual  Graph 
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Kindi's  program  then  uses  path  annealing  to  perturb  the  window 
sequence  by  randomly  re-routing  path  sequences  through  neighboring 
regions  in  an  attempt  to  find  a  better  path. 

Eligible  neighboring  regions  are  limited  by  a  bounding  ellipse  which  is  an 
oval  drawn  aroimd  the  start  and  goal.  Regions  or  portions  of  regions  outside 
the  bounding  ellipse  are  not  considered  by  the  aimealing  process. 

While  not  guaranteed  to  find  the  globally  optimal  path,  this  procedure 
does  find  a  near  optimal  path  in  much  less  time.  Although  Kindi's 
procedure  produced  excellent  results,  its  scope  is  limited  to  two  dimensional 
path  planning.  Extension  of  Kindi's  work  to  2$  dimensional  path  planning 
will  significantly  broaden  its  applications. 
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III.  GENERAL  PROBLEM-SOLVING  STRATEGY 


To  solve  the  anisotropic  weighted  region  problem,  we  need  vehicle  and 
terrain  models,  geometric  spatial  reasoning  functions,  search  functions,  cost 
functions,  and  annealing  control  fimctions. 

Given  a  vehicle  and  map,  the  general  strategy  is: 

1.  perform  a  search  through  the  map  regions  to  create  a  region  list  which 
may  contain  an  initial  path  solution. 

2.  calculate  necessary  information  and  put  these  regions  into  a  data 
structure  called  window  list. 

3.  for  each  adjacent  pair  of  regions  in  the  window  list,  iteratively 

(a)  determine  heading  ranges  and  cost  fimctions. 

(b)  perform  a  golden  ratio  search  on  each  heading  range  combination 
and  locate  the  optimal  heading  range  combination. 

(c)  p>erform  a  golden  ratio  search  on  the  region  crossing  frontier 
defined  by  the  optimal  heading  range  combination  and  locate 
(within  tolerance)  the  optimal  crossing  point. 

4.  if  any  crossing  point  shifted  more  than  the  specified  displacement,  go 
to  3. 

5.  if  any  path  segment  headings  are  impermissible,  attempt  to  detour 
around  them  to  find  a  valid  path  segment. 

6.  use  random  annealing  in  an  attempt  to  find  a  shorter  path. 

These  steps  are  described  in  greater  detail  in  subsequent  chapters. 
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IV.  MODELING  THE  VEHICLE  AND  TERRAIN 


A.  VEHICLE 

The  vehicle  model  used  in  this  work  is  a  simplified  version  of  those  used 
by  Ross  and  Rowe  [Ref.  5]  and  Rowe  and  Kanayama  [Ref.  6].  We  are 
concerned  with  vehicle-specific  heading  limits  for  climbing,  braking,  and 
slideslope  travel.  See  Figure  4.  Given  a  vehicle’s  heading  limits  and  the 
slope  and  orientation  of  a  region,  critical  headings  can  be  computed  by  the 
following  formulas: 

critical-p  =  arccos  ((tan  limit-p)  /  (tan  slop>e)) 
critical-s  =  arcsin  ((tan  limit-s)  /  (tan  slope)) 
critical-b  =  arccos  ((tan  limit-b)  /  (tan  slope)) 

where  limit-p,  limit-s,  limit-b  are,  respectively,  the  slope  angle  thresholds 
where  the  vehicle's  uphill,  sideslope,  and  braking  limits  occur. 

Each  critical  angle  has  a  dual,  which  with  the  critical  angle  defines  a 
heading  range.  These  dual  headings  are  calculated  as  follows: 

dual-p  =  -  critical-p 

dual-s  =  7c  -  critical-s 

dual-b  =  -  critical-b 

critical-I  =  -  critical-s 

dual-1  =  critical-s  -  n 
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Figure  4.  Vehicle  Model 


There  are  four  ranges:  power-limited,  braking,  and  two  sideslope  ranges, 
one  to  the  vehicle's  left  and  the  other  to  the  right.  If  the  vehicle's  heading 
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falls  within  one  of  these  ranges,  the  corresponding  condition  applies.  See 
Figure  5.  For  example,  if  the  critical  power-limit  angle  is  20°  and  its  dual  is 
340°,  a  vehicle  travelling  at  a  heading  of  355°  is  subject  to  the  power-limited 
condition. 


Figure  5.  Critical  heading  ranges 
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These  critical  headings  will  vary  from  region  to  region  as  the  slope  varies. 
If  a  critical  heading  is  equal  to  its  dual,  the  region's  slope  is  less  than  the 
vehicle's  heading  limit  and  no  heading  restriction  exists  for  that  particular 
range.  For  example,  if  a  region's  critical  sideslope  heading  is  equal  to  its  dual 
sideslope  heading,  the  vehicle  can  freely  traverse  the  region  without  fear  of 
rolling  over. 

B.  TERRAIN 

1.  Homogeneous  Regions 

The  terrain  is  modelled  by  convex  homogeneous  polygonal  regions. 
Regions  boundaries  are  defined  by  vertices  and  each  region  possesses  uniform 
surface  condition,  slope  and  weight.  The  maps  we  used  were  manually 
constructed  and  do  not  represent  actual  terrain.  See  Figure  6. 

2.  Vertex  List 

A  vertex  is  a  point  defined  by  x,  y,  and  z  coordinates.  A  vertex  can  be 
on  the  boundary  of  more  than  one  region.  All  vertices  necessary  to  define  the 
map  are  collected  into  a  vertex  list. 

3.  Region  List 

The  region  list  is  a  collection  of  all  regions  in  the  map  and  contains 
the  vertex  list,  weight,  slope,  orientation,  and  adjacent  region  list  for  each 
region. 

4.  Frontier 

A  frontier  is  the  line  separating  two  regions.  It  is  represented  as  an 
edge  with  its  endpoints  defined  by  vertices. 
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4.  Window  list 

Once  an  initial  window  sequence  (a  list  of  regions)  is  computed,  each 
region  in  the  list  is  put  into  the  window  list  (a  linked  list).  Additional 
information  for  the  window  list  (such  as  all  critical  headings,  and  various 
"housekeeping"  variables)  is  now  calculated.  We  only  compute  these  values 
for  regions  in  the  window  list,  and  thus,  only  for  the  regions  we  will  actually 
"visit.  " 

5.  Type  1,  II,  III,  IV  Traversals 

Ross  classified  traversals  through  a  region  into  four  types: 
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A  Type  I  traversal  is  the  simplest.  It  is  merely  travel  from  one 
isotropic  region  to  another  isotropic  region.  In  other  words,  the  crossing 
point  of  the  boundary  edge  is  determined  with  respect  to  weighted  distance 
only.  There  are  no  heading  restrictions  associated  with  this  type  of  crossing. 

With  a  Type  n  traversal  the  optimal  path  must  be  altered  in  one  or 
both  regions  because  it  falls  within  a  nonpermissible  sideslope  heading  range. 
The  nonpermissible  heading  is  adjusted  to  the  nearest  critical  angle,  which 
allows  the  path  to  lie  as  close  as  possible  to  the  optimal  (but  forbidden) 
heading. 

Type  in  traversals  feature  switchbacks,  which  allow  a  vehicle  to  travel 
through  an  uphill  nonpermissible  heading  range  by  switching  between  the 
pair  of  critical  angles  that  border  the  nonpermissible  range.  The  vehicle  is 
allowed  to  make  as  many  switchbacks  as  necessary  to  traverse  the  region. 

Type  IV  involves  travelling  downslope  where  braking  is  required. 
There  are  no  heading  restrictions,  but  a  different  cost  function  (described  in 
the  next  section)  is  required  for  travel  within  the  braking  range.  See  Figure  7. 

7.  Cost  Functions 

For  Type  I  traversals,  the  cost  function  is  simply  the  region's  weight 
multiplied  by  the  Euclidean  distance  calculated  from  the  point  of  entry,  pi,  to 
the  point  of  exit,  p2. 

cl  =  weight  X  distance  (pi,  p2) 

For  Type  n  traversals,  the  cost  function  is  also  weighted  distance,  as  in 
Type  I.  However,  recall  that  the  heading  is  adjusted  to  a  critical  heading  to 
allow  passage  through  the  region. 

c2  =  weight  x  distance  (pi,  p2) 
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Figure  7.  Type  I,  II,  III,  IV  Traversals 


Calculating  Type  IQ  traversals  also  involves  a  weighted  distance  cost 
function,  but  the  distance  must  take  into  account  switchbacks.  The  distance 
for  a  switchback  traversal,  regardless  of  the  number  of  switchbacks 
encountered,  requires  calculating  the  intersection  of  two  rays.  The  first  ray 
originates  at  start  point  pi  of  the  first  segment,  swl,  with  a  direction  of  the 
critical  impermissible  heading.  The  second  ray  originates  at  the  start  point  p2 
of  the  second  segment,  swl,  with  a  direction  of  ic  +  the  dual  critical 
nonpermissible  heading. 

Call  this  point  of  intersection  i.  The  cost  is  the  weighted  distance 
from  the  start  point  of  the  first  segment  to  the  point  of  intersection  i  plus  the 
weighted  distance  from  the  point  of  intersection  i  to  the  start  point  of  the 
second  segment.  See  Figure  8. 

c3  =  weight  x  distance  (pi,  i)  +  distance  (i,  p2) 

The  cost  function  for  Type  IV  (braking)  traversals  must  take  into 
account  changes  in  elevation.  Although  our  vehicle  model  claims  no  energy 
is  expended  while  travelling  within  a  braking  region,  a  virtual  cost  is 
incurred  because  potentiad  energy  is  lost  as  a  result  of  the  vehicle's  decrease  in 
elevation  [Ref.  6].  The  cost  for  travelling  within  a  braking  region  is  thus 
calculated  as  the  change  in  elevation  from  the  point  of  entering  the  region, 
pi,  to  the  point  of  exiting  the  region,  p2.  Distance  is  not  relevant  to  this  cost 
function. 

c4  =  abs  ((elevation  pi)  -  (elevation  p2)) 
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V.  PROBLEM  SOLVING 


A.  A*  SEARCH 

1.  Edge  Dual  Graph 

Kindi's  edge  dual  graph  provides  an  excellent  means  of  locating 
feasible  window  sequences.  The  fully  connected  edge  mid-points  (nodes)  of  a 
region  provide  a  good  cost  estimate  of  traversing  a  particular  portion  of  the 
region.  See  Figure  9.  But  instead  of  constructing  a  complete  edge  dual  graph, 
we  construct  "on  the  fly."  We  only  compute  distances  for  the  region  currently 
being  expanded. 

At  this  point  of  the  search  we  do  not  take  into  account  vehicle 
heading  restrictions  (except  as  noted  in  the  next  paragraph),  but  we  do  expect 
every  region  in  the  sequence  to  be  traversable  within  at  least  one  more 
heading  range.  The  A*  search  also  avoids  traversal  of  narrow  regions  where 
all  headings  between  two  frontiers  fall  within  a  non-permissible  slideslope 
range.  See  Figure  10.  Other  regions  may  be  labelled  as  obstacles  if  their 
traversal  costs  exceed  a  pre-determined  threshold  value.  The  window 
sequence  returned  by  this  search  procedure  is  not  guaranteed  to  be  feasible. 
Although  any  single  region  in  the  sequence  may  be  traversable,  a  crossing 
point  between  two  regions  which  simultaneously  avoids  nonpermissible 
sideslope  headings  in  both  regions  may  not  exist.  See  Figure  11.  In  this  case, 
we  attempt  to  detour  around  the  impermissible  path  segment. 
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Figure  9.  A*  Search 
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direction  of  travel 


It  is  impossible  to  travel  from  the  entry  frontier  to  the  exit 
frontier  without  violating  the  sideslope  heading  restrictions. 


Figure  10.  Narrow  Region  with  Nonpermissible  Entry 
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Figure  11.  Two  Regions  with  no  Valid  Crossing  Point 


2.  Heuristic 

The  heuristic  evaluation  function  for  the  A*  search  is  the  shortest 
distance  to  the  goal.  The  distance  from  each  active  node  (midpoint  of  the 
edge)  to  the  destination  is  calculated  and  added  to  the  cumulative  cost  of 
reaching  the  node.  The  node  with  the  lowest  sum  is  selected  for  expansion. 

B.  HNDING  THE  LCXIALLY  OPTIMAL  PATH 
1.  Processing  a  Window  Sequence 

The  A*  search  returns  a  list  of  regions  within  which  a  feasible  path 
lies.  The  region  list,  start,  and  goal  are  processed  into  a  linked  list  which 
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contains  region-specific  information  such  as  critical  heading  ranges,  boundary 
edge  and  exit  point,  and  a  displacement  which  measures  the  magnitude  of  a 
point’s  movement  during  the  iterative  optimization  process. 

2.  Golden  Ratio  Search 

The  golden  ratio  search  is  used  to  approximate  the  locally  optimal 
path  through  two  adjacent  regions.  The  procedure  involves  two  path 
segments,  with  one  segment  for  each  region  being  searched.  The  outer  end 
points  of  the  path  segments  remain  fixed,  and  the  interior  point  where  the 
two  segments  meet  is  adjusted  along  the  frontier  (within  a  given  tolerance)  to 
find  the  minimum  cost.  See  Figure  12. 

The  procedure  exploits  the  convex  function  defined  by  adjusting  the 
crossing  point  along  the  frontier.  By  sampling  four  points  along  the  frontier, 
it  is  possible  to  determine  that  the  optimal  cost  lies  within  the  range  of  three 
of  these  points  and  the  search  space  beyond  these  three  points  can  be 
eliminated  from  the  search  process.  This  procedure  can  be  repeated  on  an 
increasingly  smaller  search  space  until  the  desired  accuracy  is  obtained. 
However,  the  procedure  does  reach  a  point  of  diminishing  returns  where 
additional  searching  produces  negligible  results.  We  halt  the  search  when 
improvement  is  less  than  two  percent  of  the  current  path  cost 

3.  Partitioning  the  Regions  by  Heading  Ranges. 

When  calculating  the  cost  of  traversing  adjacent  regions,  we  must 
determine  within  which  heading  range  the  vehicle  is  travelling.  The 
vehicle's  heading  determines  which  cost  function  to  use.  There  are  three 
permissible  heading  ranges:  headings  within  the  power  range,  braking  range. 
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Figure  12.  Golden  Ratio  Search 


and  isotropic  (unrestricted)  range.  We  must  also  consider  the  nonpermissible 
sideslope  heading  ranges  because  an  initial  crossing  point  may  force  a  path 
segment  to  lie  within  a  nonpermissible  heading  range.  Since  the  golden  ratio 
search  requires  a  convex  cost  function  to  approximate  the  cost  of  traversing 
two  regions,  we  must  consider  the  possible  combinations  of  heading  ranges 
(and  thus  cost  functions)  between  two  adjacent  regions.  There  are  4^  =  16 
possible  combinations.  See  Table  1. 
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TABLE  1.  HEADING  RANGE  COMBINATIONS 


For  example,  "ip"  represents  the  cost  function  for  traversing  a  region  within 
an  isotropic  heading  range  and  transitioning  to  a  jjower  heading  range  when 
crossing  into  the  next  region.  The  ranges  involving  nonpermissible  sideslope 
headings  are  assigned  a  large  value  representing  "infinity." 

We  distinguish  these  heading  range  combinations  by  computing 
transition  points  along  the  frontier.  Each  transition  point  marks  the 
transition  from  one  heading  range  (and  thus  cost  function)  to  another.  See 
Figure  13. 

The  end  points  of  the  frontier  serve  as  the  initial  and  final  transition 
points.  We  must  also  keep  track  of  which  region  (the  region  before  the 
frontier  or  after  the  frontier)  and  heading  range  the  transition  point  is 
associated  with.  We  now  find  intermediate  transition  points  along  the 
frontier,  if  any  exist.  For  the  first  region,  we  compute  a  ray  from  the  region's 
entry  point  at  all  eight  critical  headings.  For  every  ray  that  intersects  the 
frontier,  we  assign  a  transition  point  at  the  point  of  intersection.  For  the 
second  region  (beyond  the  frontier),  we  compute  "back  rays"  from  the 
region's  exit  point  at  each  critical  heading  +  n.  We  again  assign  a  transition 
point  where  any  back  ray  intersects  the  frontier. 
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Figure  13.  Partitioning  Regions  by  Heading  Ranges 


Working  from  left  to  right  along  the  frontier,  we  conduct  a  series  of 
golden  ratio  searches.  Each  time  we  encounter  a  transition  point,  we  change 
to  the  appropriate  cost  fimction. 
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After  one  sweep  of  the  frontier,  we  have  determined  which  heading 
range  combination  is  likely  to  contain  the  optimal  crossing  point.  Since  we 
know  between  which  two  transition  points  this  optimal  crossing  point  lies 
(and  thus  the  appropriate  cost  function  to  apply),  we  conduct  a  more  precise 
golden  ratio  search  along  the  frontier  between  these  two  transition  points. 
This  search  locates  (within  tolerance)  the  optimal  crossing  point  between  the 
two  regions. 

C  SIMULATED  ANNEALING 
1.  Backgrotmd 

Johnson,  et  al,  [Ref.  7],  describe  the  use  of  simulated  annealing  for 
solving  the  graph  partitioning  problem.  This  is  essentially  a  modified 
technique  of  iterative  improvement  of  a  local  solution  until  an  acceptable 
result  is  achieved.  Annealing  allows  for  occasional  acceptance  of  a 
degenerative  solution  in  an  attempt  to  escape  from  a  locally  optimal  but 
globally  poor  solution. 

The  basic  simulated  annealing  algorithm  requires  as  input: 

1)  the  solution  space 

2)  control  parameters  for  the  armealing  process,  which  include 

a)  To  -  the  initial  value  of  the  control  temperature  T 

b)  Tf  -  the  "freezing"  value  of  T 

c)  R  -  the  reduction  factor  for  T  (typically  0.70  <=  R  <=  0.99) 

d)  L  -  the  maximum  number  of  attempted  moves  at  each  value  of  T 

e)  Ls  -  the  maximum  number  of  accepted  moves  at  each  value  of  T 
Beginning  with  the  initial  solution,  the  move  generator  randomly 

perturbs  this  solution  to  obtain  a  new  one.  The  change  in  cost,  Ac,  is  the 
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difference  between  the  cost  of  the  new  solution  and  the  current  solution.  If 

Ac  <  0,  the  new  cost  is  less  than  the  current  cost,  and  the  new  solution 

becomes  the  current  solution.  If  Ac  >  0,  the  new  solution  is  accepted  as  the 

current  solution  based  on  a  probability  function.  The  typical  probability 
-Ac 

function  is  P  =  e  T  ,  where  Ac  is  the  cost  difference  and  T  is  the  current 
temperature. 

The  control  temperature  T  is  expressed  in  the  same  units  as  the  cost 
function.  As  the  solution  space  is  searched,  T  is  reduced  by  the  reduction 
factor  R  which  progressively  reduces  the  probability  of  accepting  a  solution 
with  higher  cost. 

At  each  temperature  T,  the  move  generator  attempts  up  to  L  moves, 
accepting  up  to  Ls  solutions  of  higher  cost.  The  temperature  eventually 
reduces  to  Tf,  the  freezing  temperature,  where  no  solutions  with  higher  costs 
are  accepted.  At  this  point  the  algorithm  halts  and  returns  the  best  solution 
found. 

Kindi  [Refs.  4,  8]  used  this  annealing  concept  in  attempting  to 
improve  a  current  path  solution.  Recall  that  a  path  lies  in  a  window 
sequence.  The  border  between  windows  is  an  edge  whose  endpoints  are 
defined  by  vertices.  A  vertex  may  define  the  endpoint  of  more  than  one  edge. 
Thus,  a  vertex  may  be  common  to  more  than  one  window  (or  region). 

Kindi’s  procedure  randomly  selects  a  vertex,  called  the  rotation 
vertex,  which  belongs  to  the  current  window  sequence.  All  edges  with  this 
vertex  as  an  endpoint  are  identified.  The  current  path  solution  passes 
through  at  least  one  of  these  edges.  Edges  through  which  the  path  crosses  are 
removed  from  the  window  sequence  and  replaced  by  the  edges  through 
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which  the  path  does  not  pass.  See  Figure  14.  This  generates  a  path  through 
the  new  window  sequence  which  is  iteratively  improved  (using  the  golden 


Figiure  14,  Rotation  Vertex 

ratio  search)  to  achieve  a  locally  optimal  solution.  If  this  path  solution  is 
better  than  the  previous  solution,  the  new  path  is  accepted.  If  the  new  path 
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solution  is  worse  than  the  previous  solution,  the  decision  to  accept  it  is 

-Ac 

determined  by  the  probability  function  P  =  e  T  This  allows  a  means  of 
escaping  from  a  possible  local  minimum  and  searching  other  areas  of  the 
map  for  a  better  solution.  See  Figure  15.  Kindi  used  the  following  control 
parameters: 

To  =  initial  path  cost 
Tf  =1.0 
R  =.80 
L  =5 
Ls  =4 


Figure  15.  The  Cost  Function  for  Multiple  Window  Sequences 


2.  Path  Annealing  for  Anisotropic  Regions 

The  control  mechanism  Kindi  used  will  also  work  for  anisotropic 
path  annealing.  Temperature  will  control  the  program's  probability  of 
accepting  worse  or  degenerative  solutions. 
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The  process  which  perturbs  the  path,  or  makes  a  "detour,"  is  more 
complicated.  A  simple  replacement  of  edges  based  on  a  rotation  vertex  may 
not  produce  a  feasible  window  sequence.  We  must  check  the  new  edges  to 
determine  whether  we  can  travel  between  them  at  a  valid  heading. 

To  do  this,  we  select  a  detour  region.  We  designate  the  entry  point 
into  the  detour  region  as  the  detour  start  point.  We  assign  the  exit  frontier  of 
the  detour  region  as  the  detour  frontier.  The  exit  point  of  the  region 
immediately  beyond  the  detour  region  is  assigned  as  the  detour  goal  point. 
We  now  conduct  an  A*  seairch  similar  to  the  search  performed  to  produce  the 
initial  window  list.  The  search  begins  at  the  detour  start  point  and  terminates 
under  one  of  two  conditions:  1)  the  detour  goal  point  is  reached,  or  2)  an 
upstream  region  is  reached.  An  upstream  region  is  a  region  in  the  window 
list  between  the  detour  region  and  the  (global)  goal  region.  See  Figure  16. 

For  the  A*  search  we  use  frontier  endpoints  in  addition  to  frontier 
midpoints  as  nodes.  This  gives  us  greater  resolution  and  often  results  in  a 
shorter  detour.  This  procedure  is  particularly  useful  when  searching  across 
regions  that  vary  greatly  in  width. 

This  A*  search  is  more  restrictive  than  the  search  used  to  find  the 
initial  region  list.  In  addition  to  checking  for  valid  heading  ranges  between 
regions,  we  limit  the  length  of  the  detour  to  no  more  than  three  regions. 
This  "tames"  the  search  and  prevents  it  from  producing  a  circuitous  detour 
which  would  likely  result  in  a  path  solution  considerably  longer  than  the 
current  solution. 
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Figure  16.  Finding  a  Detour 


This  procedure  effectively  eliminates  many  regions  as  candidate 
detour  regions.  This  is  desirable,  since  we  don't  want  to  waste  our  time  on 
detours  unlikely  to  produce  a  better  path  solution. 

The  A*  search  produces  a  detour  region  list.  The  detour  region  list  is 
processed  into  a  window  list  and  inserted  into  the  current  window  list, 
replacing  the  detour  region(s).  The  resulting  window  list  will  contain  at  least 
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one  different  region  where  the  detour  occurs,  but  it  may  contain  up  to  two 
new  regions. 

The  golden  ratio  search  is  iteratively  applied  to  the  new  window  list 
to  determine  a  locally  optimal  (within  tolerance)  path  solution.  The  frontier 
crossing  points  of  the  windows  retained  from  the  previous  window  list  serve 
as  a  starting  solution  for  this  next  search.  The  result  is  a  faster  search,  since  a 
locally  optimal  solution  already  exists  for  path  segments  before  and  beyond 
the  detour  region(s). 

Once  the  locally  optimal  path  is  found,  we  compare  it  to  the  current 
best  path.  If  the  current  path  is  shorter,  it  becomes  the  best  path.  If  the 
current  path  is  longer  than  the  best  path  and  not  accepted  by  the  annealing 
control  mechanism,  the  previous  window  list  is  restored  and  a  new  region  is 
randomly  selected  for  a  detour. 

The  number  of  detours  attempted  is  determined  by  a  loop  structure 
and  the  preset  value  L,  and  the  maximum  number  of  accepted  moves  any  a 
given  temperature  is  Lg. 

We  initially  use  the  probability  function  mentioned  previously, 
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P  =  e  T  .  We  use  Kindi's  values  for  control  parameters,  except  for  R,  where 
we  use  .70. 

Since  our  relatively  simple  map  provides  few  opportunities  to  exploit 
path  annealing,  we  also  experiment  with  a  linear  probability  function 
P  =  T.  The  number  of  detour  attempts  allowed  is  a  function  of  the  length  of 
the  window  list.  We  also  discard  the  value  Lg,  which  governs  the  maximum 
number  of  accepted  moves  at  a  given  value  of  T.  The  temperature  T  is 
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reduced  by  a  constant  R  (i.e.  T  =  T  -  R)  after  every  L  detour  attempts.  The 
results  are  compared  in  Chapter  VI. 

We  always  keep  track  of  the  current  path  solution  and  the  best  path 
solution.  We  also  maintain  a  path  list.  A  new  path  solution  is  added  to  the 
path  list  if  it  is  accepted  as  the  current  solution.  The  path  list  allows  us  to 
provide  alternate  path  solutions  which  may  be  longer  than  the  best  path,  but 
are  nevertheless  feasible  path  solutions. 
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VI.  EMPIRICAL  STUDIES 


A.  INPUT  MAPS  AND  VEHICLES 

1.  Maps 

Our  input  map,  derived  from  Ross,  is  not  particularly  suited  to 
exploiting  the  annealing  process.  The  regions  are  largely  symmetrical  and 
oblong.  This  arrangement  often  permits  the  A*  search  to  produce  the 
optimal  window  list  instead  of  relying  on  the  annealing  process  to  find  it.  In 
this  case,  most  of  the  effort  is  spent  in  merely  confirming  that  we  have 
already  located  the  best  path.  However,  there  are  some  cases  where  annealing 
produces  a  better  solution.  In  our  test  cases,  we  display  the  path  solution 
produced  by  the  A*  search  as  well  as  the  solution  found  by  annealing. 

We  use  two  maps.  The  maps  appear  identical,  but  the  second  map 
contains  regions  weighted  in  proportion  to  their  slopes.  All  regions  in  the 
first  map  have  a  weight  of  1.  Figures  17  and  18  are  included  to  show  map 
features.  Figure  17  shows  a  path  found  by  the  A*  search.  Figxire  18  shows  the 
path  solution  after  annealing.  Heading  range  indicators  for  the  vehicle  are 
drawn  at  each  point  where  the  path  crosses  a  frontier. 

2.  Vehicle 

We  use  three  vehicles  for  the  evaluation.  Vehicle-1  and  vehicle-3  are 
adapted  from  Ross  [Ref.  1]  and  represent,  respectively,  an  armored  personnel 
carrier  and  a  cargo  truck.  Vehicle*2  caimot  climb  as  steep  a  slope  as  vehicle-1, 
but  its  sideslope  stability  is  greater.  We  did  this  to  see  how  much  the  path 
between  a  common  start  and  goal  varies  for  different  vehicles. 
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Figure  18.  Path  after  Annealing 
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B.  TEST  PROCEDURES 


A  direct  comparison  of  our  implementation  to  Ross's  is  not  possible  since 
Ross  used  Flavors  on  a  Symbolics  LISP  machine  while  we  use  Allegro 
Common  LISP  on  a  Solbourne  workstation.  We  are  not  certain  whether 
Ross's  time  computations  include  all  overhead,  output,  and  display 
procedures.  Also,  our  map  is  slightly  modified  from  Ross's.  The  slope  of 
some  regions  was  changed  to  be  more  consistent  with  vertex  elevations.  All 
regions  in  Ross's  map  are  equally  weighted,  therefore  a  comparison  of  our 
performance  with  the  weighted  map  would  be  invalid. 

We  do  observe  the  performance  of  our  implementation  based  on 
different  vehicles,  temperature  cooling  schedules,  and  maps.  For  our  time 
computation  (msec),  we  include  all  processes,  beginning  with  input  of  the 
start  and  goal  points.  We  also  include  time  allocated  to  updating  displays  and 
input/output. 

We  evaluate  the  path  annealing  process  with  two  different  probability 

-AC 

functions,  the  cost-based  exponential  function  P  =  e  T  ,  and  the  linearly 
decreasing  function  P  =  T.  Table  2  shows  the  results  of  this  comparison  for 
vehicle-1  and  Table  3  shows  the  results  for  vehicle-2.  The  temperature  and 
the  initial  path  cost  in  the  exponential  function  influences  the  number  of 
detours  attempted  as  well  as  the  probability  of  accepting  a  degenerative  path 
solution.  In  the  linearly  decreasing  function,  the  number  of  detours 
attempted  is  dependent  on  length  of  the  window  list  and  the  probability  of 
accepting  a  degenerative  path  solution  begins  with  a  constant  initial 
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temperature  and  decreases  at  a  constant  rate  during  each  iteration  until  it 
reaches  the  freezing  temperature. 


TABLE  2.  EQUALLY  WEIGHTED  REGIONS  WITH  VEHICLE-1 


vehicle-1 

A* 

linear  p 

exponential  p 

start 

goal 

cost 

time 

cost 

time 

cost 

time 

(221.0 

399.0) 

(260.0 

193.0) 

187.4 

29868 

161.0 

107348 

187.4 

159951 

(229.0 

127.0) 

(305.0 

344.0) 

233.2 

16850 

204.8 

34284 

204.7 

40784 

(399.0 

217.0) 

(561.0 

239.0) 

127.4 

15467 

127.4 

41599 

127.4 

64667 

(271.0 

415.0) 

(266.0 

194.0) 

208.8 

18317 

199.1 

58517 

199.1 

87466 

(77.0 

339.0) 

(364.0 

297.0) 

222.8 

23917 

222.8 

100617 

2218 

144850 

TA^.LE  3.  EQUALLY  WEIGHTED  REGIONS  WITH  VEHICLE-2 


vehicle-2 

A* 

linear  p 

exponential  p 

start 

goal 

cost 

time 

cost 

time 

cost 

time 

(221.0 

399.0) 

(260.0 

193.0) 

127.7 

35483 

109.7 

82200 

109.7 

217533 

(229.0 

127.0) 

(305.0 

344.0) 

156.3 

20600 

156.3 

39366 

156.3 

115684 

(399.0 

217.0) 

(561.0 

239.0) 

89.0 

17434 

51.15 

103717 

80.5 

107083 

(271.0 

415.0) 

(266.0 

194.0) 

176.7 

21366 

176.7 

39700 

176.7 

88450 

<77.0 

339.0) 

(364.0 

297.0) 

222.8 

24199 

217.9 

63501 

222.8 

179266 

We  also  tested  the  annealing  process  on  the  weighted  region  map.  The 
results  are  shown  in  Tables  4  and  5. 
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TABLE  4.  WEIGHT  PROPORTIONAL  TO  SLOPES  WITH  VEHICLE-1 


vehicle-1 

A* 

linear  p 

exponential  p 

start 

goal 

cost 

time 

cost 

time 

cost 

time 

(221.0 

399.0) 

(260.0 

193.0) 

28Z4 

27550 

259.1 

16517 

253.1 

201034 

(229.0 

127.0) 

(305.0 

344.0) 

238.6 

10516 

238.6 

16001 

238.6 

30100 

(399.0 

217.0) 

(561.0 

239.0) 

109.7 

31466 

109.7 

14900 

109.7 

52066 

(271.0 

415.0) 

(266.0 

194.0) 

328.8 

17834 

328.8 

38183 

328.8 

69000 

(77.0 

339.0) 

(364.0 

297.0) 

433.4 

20889 

433.4 

96483 

433.4 

46949 

TABLE  5.  WEIGHT  PROPORTIONAL  TO  SLOPE  WITH  VEHICLE-2 


vehicle-2 

A* 

linear  p 

exponential  p 

start 

goal 

cost 

time 

cost 

time 

cost 

time 

(221.0 

399.0) 

(260.0 

193.0) 

281.1 

30449 

216.0 

158816 

278.4 

229483 

(229.0 

127.0) 

(305.0 

344.0) 

238.6 

10533 

238.6 

35384 

238.6 

62167 

(399.0 

217.0) 

(561.0 

239.0) 

79.1 

38233 

79.1 

44634 

79.1 

90000 

(271.0 

415.0) 

(266.0 

194.0) 

347.8 

20966 

347.8 

29784 

347.8 

68850 

(77.0 

339.0) 

(364.0 

297.0) 

445.6 

20617 

445.6 

46234 

445.6 

%284 

C  ANALYSIS  OF  TEST  RESULTS. 

The  results  support  the  conclusion  that  path  annealing  can  improve 
upon  an  initial  A*  search  solution  in  the  2^  dimensional  weighted  region 
problem.  However,  the  stochastic  approach  may  possess  disadvantages. 
Because  detours  are  chosen  randomly,  we  are  not  guaranteed  of  achieving  the 
same  solution  for  multiple  runs  of  the  program.  Even  if  multiple  runs 
produce  the  same  solution,  we  don't  know  when  the  best  solution  was  found. 
It  may  be  found  on  the  first  detour,  or  it  may  be  found  only  after  making 
many  detours. 
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If  time  is  limited,  we  should  be  able  to  halt  the  program  prematurely  and 
return  the  current  best  solution.  The  randomness  of  choosing  a  detour 
region  virtually  assures  that  a  program  repeatedly  halted  at  the  same  point 
will  return  different  current  solutions  for  the  same  start  and  goal. 

A  possible  cure  for  this  is  to  select  a  detour  region  based  on  a  heuristic 
evaluation.  We  should  attempt  to  detour  through  regions  with  a  high 
potential  for  reducing  the  current  path  solution.  A  relatively  high  cost  region 
or  a  path  segment  with  a  sharp  change  in  direction  may  be  promising 
candidates  for  a  detour.  This  area  warrants  further  study. 

In  our  test  cases,  the  linearly  decreasing  probability  function  works  as  well 
as  the  exponential  probability  function  as  far  as  finding  the  best  path. 
Execution  time  is  less  for  the  former,  however,  this  observation  is  based  on 
very  limited  testing  and  is  probably  dependent  on  the  shapes  and  sizes  of  map 
regions. 
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VII.  SUMMARY  AND  CONCLUSIONS 


Finding  the  optimal  path  solution  through  complex  and  varied  terrain  is 
expensive  in  terms  of  time  and  computer  resources.  Often  heuristics  can  be 
applied  to  find  a  near-optimal  or  otherwise  acceptable  path  solution  in  much 
less  time.  Several  efforts  have  been  made  to  locate  feasible  paths  through 
two-dimensional  terrain.  This  work  expands  Kindi's  stochastic  path 
planning  approach  to  the  2^  dimensional  weighted  region  problem. 

It  is  important  to  begin  with  a  good  initial  window  list.  This  is 
accomplished  by  an  A*  search.  The  effectiveness  of  path  annealing  is 
probably  dependent  on  characteristics  of  the  terrain,  but  further  study  is 
needed  in  this  area.  Random  annealing  works,  but  a  heuristic  approach  to 
annealing  should  be  explored.  For  example,  it  may  prove  beneficial  to  detour 
around  high  cost  regions. 

To  be  certain  of  finding  the  globally  optimal  solution,  one  must 
exhaustively  search  every  feasible  window  sequence.  Ross  shows  that  this 
can  be  prohibitively  expensive  in  terms  of  time  and  computational  effort.  A 
heuristic  and  stochastic  approach  produces  a  timely  and  acceptable  path 
solution,  but  random  detours  will  not  always  return  the  same  path  solution 
for  a  given  stcirt  cind  goal. 

The  basic  concept  is  shown  to  work  in  the  LISP  implementation.  We  do 
not  guarantee  an  optimal  or  near-optimal  path  solution  will  be  found.  We 
do  begin  with  a  feasible  starting  solution  and  iteratiavely  attempt  to  improve 
it  under  the  control  of  the  golden  ratio  search  tolerances  and  the  annealing 
temperature. 
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Suggested  extensions  to  this  research  include  testing  and  evaluating  the 
performance  of  this  implementation  on  various  terrain  models, 
incorporating  a  terrain  mapping  program,  developing  heuristic  evaluation 
functions  for  selecting  detour  regions,  and  the  use  of  parallel  processing  to 
simultaneously  compute  multiple  window  sequences. 
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APPENDIX  A.  NOTES  ON  THE  IMPLEMENTATION 


1.  The  implementation  program  was  written  in  Common  LISP  with  X 
Windows  and  run  on  a  Solbume  Workstation.  The  system  possesses 
its  own  operating  system,  16  Megabytes  of  RAM,  and  runs  at  a  clock 
speed  of  33  MHz. 

2.  The  emphasis  was  on  producing  a  working  prototype  with  efficiency  a 
secondary  priority.  Many  of  Ae  routines  used  in  this  program  were 
more  efficiently  implemented  using  C++  and  Prolog  by  Kindi.  We 
perform  very  little  pre-processing  on  the  map  data,  preferring  instead 
to  calculate  as  we  conduct  the  search.  In  an  application  where  one  map 
is  extensively  used,  pre-processing  would  undoubtedly  prove  more 
efficient. 

3.  We  used  only  the  simplest  data  structures.  The  program  is  essentially 
constructed  with  LISP's  defstruct,  cond,  if,  dolist,  cons,  append 
functions.  Sequential  doubly  linked  lists  were  used  instead  of  arrays, 
hash  tables,  or  binary  trees.  The  intent  was  to  produce  simple  and 
readable  code  (as  far  as  this  is  possible  in  LISP). 

4.  Local  variables  were  primarily  used.  We  only  used  global  variables  for 
such  data  as  the  start/ goal  points,  window  lists,  region  lists,  accuracy 
control  variables,  and  annealing  control  variables. 

5.  There  is  no  elaborate  user  interface.  The  user  can  use  the  setf 
command  to  change  vehicles.  The  start  and  goal  points  are  input  by 
using  the  mouse. 
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APPENDIX  B.  SOURCE  CODE  (COMMON  LISP) 


Filename:  start. cl 
By:  HILTON,  Cary  A. 
Date:  May  91 


Function:  Startup  file  for  anisotropic  path  planning. 

1.  creates  and  activates  window  *map* 

2.  loads  file  "find. cl" 

3.  to  tagin,  type  "(path)",  then 
click  left  mouse  button  on 
start,  goal  points 


(use-package  :cw) 

(recjuire  :xcw) 

( initialize-common-windows ) 

(defvar  *map*) 

(defvar  *first-win*) 

(defvar  *win-num*) 

(defvar  *x-min*  0) 

; (defvar  *x-max*  695) 
(defvar  *x-max*  600) 

(defvar  *y-min*  25) 

; (defvar  *y-max*  838) 
(defvar  *y-max*  830) 


(setf  *map* 

(make-window-stream 


:  left 

*x-min* 

:  bottom 

*y-min* 

:  width 

*x-max* 

:  height 

*y-max* 

:  title 

"MAP  WINDOW" )  ) 

(activate  *map*) 
(load  "find. cl") 
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Filename:  find .cl 
By:  HILTON,  Cary  A. 

Date:  May  91 

Function:  Control  structures  for  program. 

1.  loads  all  files  necessary 

to  ran  program  (except  "start. cl") 

2 .  sets  default  maps  and  vehicles 

3.  contains  top  level  control  program 


(load  "anneal. cl") 

(load  "converge. cl") 

^oad  "costf.cl") 

(load  "drawf.cl") 

(load  "geometry. cl") 

(load  "linlcs.cl") 

(load  "part it ion. cl") 

^oad  "search. cl") 

^oad  "vehicle. cl") 

(load  "map2.cl") 

;  commands  to  load  compiled  files 

; (load  "anneal .fasl") 

; (load  "converge .fasl") 

; (load  "costf . fasl") 

; (load  "drawf . fasl") 

; (load  "geometry . fasl") 

;  (load  "lin)cs.fasl") 

;  ^oad  "partition . fasl") 

;  ^oad  "search. fasl") 

/(load  "vehicle. fasl") 


/(load  "map2.fasl") 

^ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ 


(defstruct  path 
region 
point 
heading 
cost) 

(defvar  *start*) 
(defvar  *goal*) 
(defvar  *rlist*) 
(defvar  *winlist*) 
(defvar  *init-win*) 


/  global  start  point 
/  global  goal-point 
/  path  passes  through  region  list 
/  processed  window  list 
/  first  window  in  window  list 
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(defvar  *first-win*) 


;  second  window  in  window  list 


(defvar  *path-list*  nil) 

(defvar  *current-path*  nil) 

(defvar  *best-path*  nil) 

(defvar  *new-path*  nil) 

(defvar  *random-value*) 

(defvar  *accept-path*  .5)  ;  this  is  the  annealing  "temperature” 

;  when  temperature  reaches  0,  no 
;  degenerate  paths  will  be  accepted 

(defvar  *l-move*  4) 

(defvar  *l-accept*  4) 

(defvar  *reduct ion-factor*  0.7) 

(defvar  *temp-f*  1.0) 

(def constant  *e*  2.71828) 

f 

;  Default  vehicle  is  vehicle-1 

(setf  *vehicle*  (vehicle-rad  vehicle-1)) 

; (setf  *vehicle*  (vehicle-rad  vehicle-2)) 

; (setf  *vehicle*  (vehicle-rad  vehicle-3) ) 

; (load  "map3.ci") 

; (load  "map3.fasl") 

/ 

(defun  get-start  () 

(let  (mouse-p) 

(pprint  "clic)c  mouse  on  start  point") 

(setf  mouse-p  (get-position  *map*)) 

(setf  *start*  (cons  (float  (position-x  mouse-p) ) 

(list  (float  (position-y  mouse-p))))) 
(draw-start  *start*) 

(pprint  *start*))) 

(defun  get-goal  () 

(let  (mouse-p) 

(pprint  "clic)c  mouse  on  goal  point") 

(setf  mouse-p  (get-position  *map*)) 

(setf  *goal*  (cons  (float  (position-x  mouse-p)) 

(list  (float  (position-y  mouse-p))))) 

(draw-goal  *goal*) 

(pprint  *goal*))) 

(defun  create-window-list  (start  goal) 

(setf  *winlist*  (change-struct  start  goal  *rlist*) ) 

(double-link  (first  *winlist*)  (rest  *winlist*)) 

(setf  *init-win*  (first  *winlist*) ) 


(setf  *first-win*  (segment -next -win  *init-win*)) 

(setf  *win-num*  (segment-index  (first  (last  *winlist*) ) ) ) ) 

(defun  path  () 

(setf  *best-path*  nil) 

(setf  *path-list*  nil) 

(setf  *current-path*  nil) 

(clear  *map*) 

(draw-regions  *global-region-list*) 

;  (get-start) 

;  (get -goal) 

(find-path  *start*  *goal*) 

(loop 

(cond  (*bad-segments* 

(detour-at  (first  *bad-segments*) ) 

(qdraw) 

(converge  *f irst-win*) ) ) 

;  (draw-final  *f irst-win*) 

(if  (null  *bad-segments*) 

(return) ) ) 

(setf  *current-path*  (store-path  *first-win*) ) 

(setf  *best-path*  *current-path*) 

(setf  *path-list*  (append  *path-list* 

(list  * current -pat h*) ) ) 

;  (anneal-path)  ;  linear  cooling 

;  (anneal-path2)  ;  exponential  cooling 

nil) 

(defun  anneal-path  () 

(let  ( (detour-region)  (attempts)  ;  "attempts"  is  assigned  the 
nunfcer 

(detour-count  0)  (move-prob)  ;  of  regions  in  the  region  list 
(visited) ) 

(loop 

(setf  attempts  (segment -index 

(first  (last  *winlist*) ) ) ) 

(setf  detour-region  (random-region) ) 

(pprint  "detour-region; ") (pprint  detour-region) 

(unless  (memberp  detour-region  visited) 

(detour-at  detour-region) 

(cond  (*detour-rlist* 

(qdraw) 

(converge  *first-win*) 

(draw-final  *first-win*) 

(setf  *current-path*  (store-path  *first-win*) ) 

(setf  *accept-path* 

(-  *accept-path*  0,1)) 

(cond  ((or  (null  *best-path*) 

(<  (first  *current-path*) 

(first  *best-path*) ) ) 

(setf  *best-path*  *current-path*) 

(setf  *path-list*  (append  *path-list* 

(list  *current-path*) ) ) ) 

(t 

(setf  move-prob  (/  (random  100)  100)) 
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(if  (or  (>  (first  *current-path*) 
♦infinity*) ) 

(>  move-prob  *accept-path*) ) 
(restore-path) ) ) ) ) 

(setf  visited  (append  visited 

(list  detour-region) ) ) )  ;  end  unless 
(incf  detour-count) 

(if  (>“  detour-count  atten^ts) 

(return) ) )  ;  end  loop 
(draw-best -path  *best-path*) 

(print-stored-costs  *path-list*) ) )  ;  end  let,  defun 


(defun  anneal-path2  () 

(let  ( (tenip  (first  *hest-path*} } 

(accepted  0)  (prob) 

(detour-region)  (visited) 

(delta-c)  (decision-point)) 

(unless  (>  *teitp-f*  temp) 

(loop 

(dotimes  (index  *l-move*) 

(setf  detour-region  (random-region)) 

(unless  (memberp  detour-region  visited) 

(detour-at  detour-region) 

(cond  (*detour-rlist* 

(qdraw) 

(converge  *first-win*) 

(draw-final  *first-win*) 

(setf  *current-path*  (store-path  *f irst-win*) ) 

(setf  delta-c  (-  (first  *current-path*) 

(first  *best-path*) ) ) 

(cond  ((and  (<  accepted  *l-accept*) 

(<  delta-c  0) ) 

(incf  accepted) 

(setf  *path-list* 

(append  *path-list* 

(list  *best-path*) ) ) 

(setf  *best-path*  *current-path*) ) 

(t 

(setf  prob 

(expt  *e*  (-  (/  delta-c  temp)))) 

(setf  decision-point  (/  (random  100)  100)) 

(if  (or  (>  (first  *current-path*) 

♦infinity*) 

(>  decision-point  prob) ) 

(restore-path) ) ) ) ) )  ;  end  cond 
(setf  visited  (append  visited 

(list  detour-region)))))  ;  end  unless. 


dotimes 

(setf  temp  (♦  temp  *reduction-factor*) ) 

(if  (<=  temp  *temp-f*) 

(return) ) ) )  ;  end  loop,  unless 
(draw-best-path  *best-path*) 

(print-stored-costs  *path-list*) ) )  ;end  let,  defun 


(defun  find-path  (start  goal) 
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(let  ( (start-region  (find-region  *start*) ) 

(goal-region  (find-region  *goal*) ) 

(path-cost) ) 

(pprint  "start -region;  ") (pprint  start-region) 

(pprint  "goal-region:  ") (pprint  goal-region) 

(cond  ( (eq  start-region  goal-region) 

(setf  *rlist*  (list  start-region)) 

(pprint  "*rlist*:  ") (pprint  *rlist*) 

(create-window-list  start  goal) 

(setf  path-cost 

(check-direct-path  start  goal  *first-win*) ) 

(unless  (null  path-cost) 

(setf  (segment -cost  *first-win*) 
path-cost) 

(pprint  "heading;  ") 

(pprint  (rad-to-deg  (angle  (cons  *start* 

(list  *goal*))))) 

(pprint  "path-cost:  ") 

(pprint  path-cost) 

(draw-direct  ’'start*  *goal*  *first-win*)  ) )  ) 

(cond  ((null  path-cost) 

(setf  *rlist*  (region-search  start  goal 

start-region 
goal-region) ) 

(cond  (*rlist* 

(pprint  "*rlist*:  ") (pprint  *rlist*) 
(create-window-list  start  goal) 

(qdraw) 

(converge  *first-win*) 

(draw-final  *first-win*) ) 

(t 

(pprint  "no  solution")))))))  ;  end  cond,  cond  let, 

defun 

(defun  check-direct-path  (pi  p2  current) 

(let*  ( (edge-t  (cons  pi  (list  p2))) 

(hdg-t  (angle  edge-t))) 

(cond  ( (inside-rb  hdg-t  (segment-critical-b  *first-win*) 
(segment-dual-b  *first-win*) ) 

(b-cost  pi  p2  nil  nil) ) 

((inside-pl  hdg-t  (segment-critical-p  *first-win*) 

( segment -dual-p  *first-win*) ) 

(p-cost  (pi  p2  *first-win*) ) ) 

((inside-pl  hdg-t  (segment-critical-1  *first-win*) 

(segment -dual-1  *first-win*) ) 

nil) 

((inside-rb  hdg-t  (segment-critical-r  *first-win*) 
(segment-dual-r  *first-win*) ) 

nil) 

(t 

(i-cost  pi  p2  *first-win*) ) ) ) ) 

(defun  store-path  (init) 

(let  ((current  init) 
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(path-cost  0) 

(temp)  (path-list) ) 

(loop 

(setf  temp  (make-path 

; region  (segment-region  current) 

: point  (segment -exit -point  current) 

: heading  (segment-heading  current) 

:cost  (segment-cost  current))) 

(setf  path-cost  (+  path-cost  (segment -cost  current))) 
(setf  path-list  (append  path-list  (list  teiip) ) ) 

(setf  current  (segment-next -win  current)) 

(if  (null  current) 

(return) ) ) 

(append  (list  path-cost)  path-list))) 

(defun  print-best-path  (path-list) 

(let*  ((cost  (first  path-list)) 

(segments  (rest  path-list) ) ) 

(dolist  (element  segments) 


(pprint  " - ") 

(pprint  "point:")  (pprint  (path-point  element)) 
(pprint  "heading:")  (pprint  (path-heading  element)) 
(pprint  "cost:")  (pprint  (path-cost  element))) 

(pprint  " - ") 

(pprint  "path-cost:")  (pprint  cost) 

(pprint  " - "))) 


(defun  print-stored-costs  (path-list) 


(pprint  " - ") 

(pprint  "alternate  path  costs:") 

;  (pprint  " - ") 

(cond  (path-list 


(dolist  (path  path-list) 

(setf  segments  (rest  path)) 

(dolist  (element  segments) 

(pprint  " - ") 

(pprint  "point:")  (pprint  (path-point  element)) 

(pprint  "heading:")  (pprint  (path-heading  element)) 

(pprint  "cost:")  (pprint  (path-cost  element))) 

(pprint  " - ") 

(pprint  "path-cost:")  (pprint  (first  path)))) 

(t 

(pprint  "no  improvements  found")))) 
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Filename;  anneal. cl 
By:  HILTON,  Cary  A. 

Date:  May  91 

Function:  Performs  path  annealing. 

1.  detours  around  path  segments 

with  an  impermissible  heading 

2 .  performs  random  path  annealing 

3.  uses  A*  search  from  a  detour  start  point 
to  a  detour  goal  point 

4 .  detour  path  cannot  pass  through  more  than 
3  regions 

★  ★★★★★★★★*★★★*■*■★★★★★★★★★***★★★★★****★****★***★*★★★**★*★★******★* 

(defvar  *detour-rlist*  nil) 

(defvar  *detour-start*  nil) 

(defvar  *detour-goal*  nil) 

(defvar  *upstrecim-regions*  nil) 

(defvar  *downstream-regions*  nil) 

(defvar  *droot*  nil) 

(defvar  *save-link-begin*) 

(defvar  *save-link-end*) 

(defvar  *restore-link-begin*) 

(defvar  *restore-link-end*) 

(defun  f ind-region-in-winlist  (target -region) 

(let  ( (current -window  *f irst-win*) ) 

(loop 

(if  (eq  (segment-region  current-window) 
target -region ) 

(return) 

(setf  current-window  (segment-next-win 

current-window) ) ) ) 

current -window) ) 

(defun  f ind-index-in-winlist  (win-num) 

(let  ( (current -window  *f irst-win*) ) 

(loop 

(if  (eq  (segment-index  current -window) 
win-num) 

(return) 

(setf  current-window  (segment-next-win 
current -window) ) ) ) 

(segment-region  current -window) ) ) 
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(defun  detour  (avoid-region) 

(let  ( (start-region)  (goal-region) 

(detour-start)  (detour-goal) 

(avo id-edge) 

(window) ) 

(setf  window  (find-region-in-winlist  avoid-region)) 

(setf  detour-start  (segment-exit -point 

(segment -prior-win  window))) 

(setf  detour-goal  (segment -exit-point 
(segment-next -win  window) ) ) 

(setf  start-region  (segment -region  window) ) 

(setf  goal-region  (segment -region 

(segment-next -win  window))) 

(setf  avoid-edge  (segment-frontier  window)) 

(setf  *upstream-regions*  (get -upstream  avoid-region  *rlist*)) 
(setf  *downstream-regions*  (get-downstream  avoid-region 
*rlist*) ) 

(setf  *detour-rlist*  (anneal-search  detour-start 

detour-goal 
start-region 
goal-region 
avoid-edge) ) ) ) 

(defun  get-downstream  (region  rlist) 

(let  (downstream) 

(loop 

(if  (or  (null  rlist) 

(eq  region  (first  rlist))) 

(return) ) 

(setf  downstream 

(append  downstream  (list  (first  rlist)))) 

(setf  rlist  (rest  rlist))) 
downstream) ) 

(defun  get-upstream  (region  rlist) 

(loop 

(if  (or  (null  rlist) 

(eq  region  (first  rlist))) 

(return) 

(setf  rlist  (rest  rlist)))) 

(rest  rlist)) 


(defun  anneal-search  (start  goal 

start-region 

goal-region 

avoid-edge) 

(let*  ( (active-region  start-region) 

(prior-edge  (cons  start  (list  start))) 
(current-edge)  (node-triple) 

(active-node)  (active-node-cost) 

(est-to-goal)  (cost-tot  0)  (temp-node) 
(prior-node  start)  (prior-region  start-region) 
(prior-cost  0) 

(bac)c-path  (list  start-region))  (rlist)) 
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; (pprint  "detour  start  point : " ) 

;  (pprint  start) 

; (pprint  "detour  start  region:") 

; (pprint  start-region) 

; (pprint  "detour  goal  point:") 

; (pprint  goal) 

; (pprint  "detour  goal  region:") 

; (pprint  goal-region) 

(setf  *droot*  nil) 

(setf  current-node  (malce-node 

: point 

:edge 

: region 
: chain 

:cost 
: cost-est 
: prior 
:next 


start 

nil 

start-region 

nil 

0 

0 

nil 
nil)  ) 


(loop 

(dolist 

(unless 


node) ) 


(element  (polygon-alist  (eval  active-region) ) ) 

(or  (>  length  (node-chain  current-node) )  2) 

(and  (eq  element  prior-region) 

(not  (eq  start-region  goal-region) ) ) 
(virtual-obstacle  element) 

(and  (memberp  element  (node-chain  current - 

(not  (eq  start-region  goal-region))) 

(memberp  element  *downstream-regions*) ) 


(setf  current-edge  (find-edge  (eval  active-region) 

(eval  element))) 

(setf  cur rent -edge 

(cons  (eval  (first  current-edge) ) 

(list  (eval  (second  current -edge) ))) ) 

(unless  (or  (null  (first  current-edge) ) 

(null  (valid-entry  prior-edge  current-edge 

active-region) ) 

(ecjual  current -edge  avoid-edge) ) 


(setf  node-triple  (append 

(list  (mid-point  (first  current-edge) 
(second  current-edge) ) ) 

(list  (first  current-edge)) 

(list  (second  current-edge) ) ) ) 

(dolist  (active-node  node-triple) 

(setf  active-node-cost 

(+  (w-distance  prior-node  active-node 

(polygon-weight  (eval  active-region) ) ) 
prior-cost) ) 

(draw-dotted  (cons  active-node  (list  prior-node) ) ) 
(setf  est-to-goal  (distance  active-node  goal)) 

(setf  cost-tot  est-to-goal) 

(setf  cost-tot  (+  active-node-cost  est-to-goal)) 
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(setf  teitp-node  (make-node 
: point 

:edge 

: region 
; chain 

:cost 
:cost-est 
: prior 
:next 

(setf  *droot*  (insert-i 

dolist 


active-node 

current-edge 

element 

back-path 

active-node-cost 
cost -tot 
nil 
nil) ) 

ie  temp-node  *droot*) )  )  ;  end 


)))  ;end  unless,  unless,  unless,  dolist 


(if  (null  *droot*) 

(return) 

(setf  current-node  *droot*)) 

(setf  back-path  (append  (node-chain  current-node) 

(list  (node-region  current-node)))) 

(setf  prior-cost  (node-cost  current-node) ) 

(if  (memberp  (node-region  current-node)  ♦upstream-regions*) 
(return) ) 

(setf  prior-edge  (node-edge  current-node) ) 

(setf  *droot*  (node-next  *droot*) ) 

(setf  prior-region  active-region) 

(setf  prior-node  (node-point  current-node) ) 

(setf  active-region  (node-region  current-node)))  ;  end  loop 

(if  (eq  (node-region  current -node)  goal-region) 

(draw-dotted  (cons  (node-point  current-node) 

(list  goal) ) ) ) 

(setf  rlist  (append  (node-chain  current-node) 

(list  (node-region  current-node) ) ) ) 

(if  (memberp  (first  (last  rlist)) 

♦upstream-regions*) 

rlist 
nil) ) ) 

(defun  detour-at  (detour-segment) 

(let  ( (detour-rlist)  (detour-winlist) 

(detour-start)  (detour-goal) 

(index)  (save-frontier)  (save-length) 

(detour-region  detour-segment) ) 

(cond  (detour-segment 

(setf  detour-rlist  (detour  detour-region) ) 

(cond  (detour-rlist 

(setf  detour-start  (find-region-in-winl Lst  detour- 

region)  ) 

(setf  detour-goal  (f ind-region-in-winlist 

(first  (last  detour-rlist) ) ) ) 

(setf  save-frontier  (segment-frontier  detour-goal)) 
(setf  save-length  (segment-frontier-length  detour- 

goal)  ) 
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(setf  *detour-winlist* 

(change-struct 

(segment -exit-point  detour-start) 

(segment -exit -point  detour-goal) 
detour-rlist) ) 

(double-link  (first  *detour-winlist*)  (rest  *detour- 

winlist*) ) 

(setf  (segment-next-win  (first  (last  *detour-winlist*) ) ) 
nil) 

(setf  (segment-frontier  (first  (last  *detour-winlist*) ) ) 
save-frontier) 

(setf  (segment-frontier-length  (first  (last  *detour- 

winlist*) ) ) 

save-length) 

(setf  *save-link-begin*  detour-start) 

(setf  *restore-link-begin*  (segment-prior-win  detour- 

start)  ) 

(setf  (segment-next-win  *restore-link-begin*) 

(second  *detour-winlist*) ) 

(setf  (segment -prior-win  (second  *detour-winlist*) ) 
*restore-link-begrn* ) 

(unless  (null  (segment-next -win  detour-goal) ) 

(setf  *save-link-end*  detour-goal) 

(setf  *restore-link-end* 

(segment -next -win  detour-goal)) 

(setf  (segment-prior-win 
*restore-link-end*) 

(first  (last  *detour-winlist*) ) ) 

(setf  (segment -next -win  (first  (last  *detour-winlist*) ) ) 
*restore-link-end*) ) 

(renumber-indices) )))))) 

(defun  renumber-indices  () 

(let  ((current  *init-win*)  (index  0)) 

(setf  *first-win* 

(segment-next -win  *init-win*)) 

(loop 

(setf  (segment-index  current)  index) 

(setf  current  (segment-next -win  current) ) 

(incf  index) 

(if  (null  current) 

(return)))))  ;end  loop,  let,  defun 

(defun  random-region  () 

(let  ( (detour-num)  (detour-region) 

(win-num) ) 

(setf  *win-num* 

(segment -index  (first  (last  *winlist*) ) ) ) 

(setf  detour-num  (random  *win-num*)) 

(if  (<  detour-num  1) 

(setf  detour-num  1) ) 

(f ind-index-in-winlist  detour-num) ) ) 

(defun  restore-path  () 

(setf  (segment -next -win  *restore-link-begin*) 
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*save-link-begin*) 

(setf  (segment -prior- win  *save-linlc -begin*) 
*restore-link -begin* ) 

(if  *restore-link-end* 

(setf  (segment -prior-win  *restore-link-end*) 
* save-link-end*) ) 

(setf  (segment -next -win  *save-link-end*) 
*restore-link-end*) 

(setf  *first-win*  (segment -next-win 
*init-win*) ) 

(renumber-indices) 

nil) 
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;  Filename :  converge . cl 

;  By:  HILTON,  Cary  A. 

/ 

;  Date:  May  91 
/ 

;  Function:  Control  structure  for  the  golden  ratio  search 

routine. 

# 

;  1)  setf  tolerances  which  determine  when  to 

;  stop  the  search 

/ 

;  2)  computes  current  path  cost 

$ 


(defvar  *edge-tolerance*  12) 
(defvar  *point-tolerance*  6) 
(defvar  ^precision* 
value 

(setf  *lcount*  6) 
allowed 

• ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ 
/ 


;  edge  length  during  golden 
;  ratio  search 
;  crossing  point  displacement 
;  used  in  "converge” 

;  halt  search  if  inprovement  of 
;  current  cost  is  less  than  this 

;  Max  number  of  outer  loops 
;  during  "converge" 


(defun  converge  (current) 

(let  ((anchored  1)  (count  0) 

(current-cost  0)  (prev-cost  *infinity*)) 

(loop 

(setf  anchored  1) 

(loop 

(unless  (or  (null  (segment-next -win  current)) 

(=  (segment-frontier-length  current)  0)) 

;  (if  (and  (=  (first  (segment-exit-point  current)) 

;  (first  (segment-exit-point  (segment-next-win 

current) ) ) ) 

;  (=  (second  (segment -exit -point  current)) 

;  (second  (segment-exit-point  (segment-next -win 

current) ) ) ) ) 

;  (snip  (segment-next -win  current))) 

(adjust-crossing  current) 

(if  (>  ( segment -displ  current)  *point-tolerance*) 

(setf  anchored  0)))  ;  end  unless 

(if  (null  (segment-next-win  current)) 

(return) 

(setf  current  (segment-next -win  current))))  ;end  loop 


60 


(incf  count) 

(setf  prev-cost  current-cost) 

(setf  current-cost  (calc-path-cost  *f irst-win*) ) 
(pprint  "path  cost") (pprint  current-cost) 

(if  (or  (plusp  anchored) 

(trivial-improvement 

prev-cost  current-cost) 

(<  *lcount*  count)) 

(return) 

(setf  current  *first-win*) ) )  ;  end  loop 

(pprint  "*lcount*") 

(pprint  count))) 

(defun  trivial-improvement  (cl  c2) 

(let  ((diff  (-  cl  c2))) 

(if  (or  (>  c2  cl) 

(<  (/  diff  cl) 

♦precision*) ) 
t 

nil) ) ) 

(defun  calc-path-cost  (element) 

(let  ( (tot  0) ) 

(loop 

(setf  tot  (+  tot  (segment-cost  element))) 

(setf  element  (segment-next -win  element) ) 

(if  (null  element) 

(return) ) ) 
tot) ) 

;  "snip"  removes  a  window  from 
;  the  window  list .  It  is 
;  not  used  in  current  version. 

(defun  snip  (element) 

(pprint  "snipping:  ") 

(pprint  (segment-region  element)) 

(if  (eq  element  *first-win*) 

(setf  *first-win*  (segment-next-win  *first-win*) ) ) 
(if  (not  (null  (segment-prior-win  element))) 

(setf  (segment-next-win  (segment-pri.or-win  element)  ) 
(segment-next-win  element))) 

(if  (not  (null  (segment -next-win  elem'^nt) ) ) 

(setf  (segment -prior-win  (segment-next-win  element) ) 
(segment-prior-win  element) ) ) ) 
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;  Filename:  costf.cl 
/ 

;  By:  HIITON,  Cary  A. 

t 

;  Date:  May  91 

;  Function:  Contains  cost  functions  for  all  possible 
;  combinations  of  heading  ranges  between 

;  two  regions. 

;  1.  gats  pre-point,  pre-range,  frontier, 

;  post-point,  post-range,  current, 

;  then  branches  to  appropriate  cost  function. 

/ 

;  2.  returns  cost,  crossing  point,  and  heading  range 

combination 
$ 
i 

(defvar  *interval*  o)  ;  used  to  space  points  along  a  segment. 


(defun  costf  (from  pre-range  edge  to  post-range  current  loop-flag) 
(cind  ((or  (eq  pre-range  's) 

(eq  post-range  ’s)) 

(hi-search  from  edge  to  current  loop-flag  pre-range  post¬ 
range)  ) 

( (eq  pre-range  'i) 

(cond  ( (eq  post-range  ’i) 

(ii-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’p) 

(ip-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’b) 

(ib-search  from  edge  to  current  loop-flag) ) ) ) 

( (eq  pre-range  'p) 

(cond  ( (eq  post-range  ’i) 

(pi-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’p) 

(pp-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’b) 

(pb-search  from  edge  to  current  loop-flag) ) ) ) 

( (eq  pre-range  'b) 

(cond  ( (eq  post-range  'i) 

(bi-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’p) 

(bp-search  from  edge  to  current  loop-flag) ) 

( (eq  post-range  ’b) 

(bb-search  from  edge  to  current  loop-flag) ) ) ) ) ) 


;  high-cost  search 
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(defun  hi-search  (from  edge  to  current  loop-flag  pre-range  post- 
range) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy)  (ranges) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min) ) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge)) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pD) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy)  ) ) ) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy) ) ) ) 

(cond  ( (eq  pre-range  *s) 

(setf  prel  (hi-cost  from  pi  current) ) 

(setf  pre2  (hi-cost  from  p2  current) ) 

(setf  pre3  (hi-cost  from  p3  current)) 

(setf  pre4  (hi-cost  from  p4  current))) 

(t 

(setf  prel  (i-cost  from  pi  current)) 

(setf  pre2  (i-cost  from  p2  current)) 

(setf  pre3  (i-cost  from  p3  current) ) 

(setf  pre4  (i-cost  from  p4  current)))) 


(cond 

( (eq  post-range  ' s) 

current) ) ) 

(setf 

postl 

(hi-cost 

pi 

to 

(segment -next -win 

current) ) ) 

(setf 

post2 

(hi-cost 

p2 

to 

(segment -next -win 

current) ) ) 

(setf 

post3 

(hi-cost 

P3 

to 

( segment -next -win 

current) ) ) ) 

(setf 

(t 

post4 

(hi-cost 

p4 

to 

(segment -next -win 

current) ) ) 

(setf 

postl 

(i-cost 

pi 

to 

(segment -next -win 

current) ) ) 

(setf 

post2 

(i-cost 

p2 

to 

(segment -next -win 

current) ) ) 

(setf 

post3 

(i-cost 

P3 

to 

( segment-next-win 

current) ) ) ) ) 

(setf 

post4 

(i-cost 

p4 

to 

( segment -next -win 

(setf  cl  (+  prel  postl)) 
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(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3) ) ) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge))) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(setf  ranges  (cons  pre-range  (list  post-range) ) ) 

(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

ranges) ) ) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

ranges ) ) ) 

( (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

ranges) ) ) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  ranges) ) ) ])) 


f 

;  isotropic-to-isotropic  search 

(defun  ii-search  (from  edge  to  current  loop-flag) 

(let*  ( (crossing-pt) 

(path-leg) 

(prel)  (pre2) 

(postl)  (post2) 

(cl)  (c2)) 

(setf  path-leg  (cons  from  (list  to) ) ) 

(setf  crossing-pt  (find-int  path-leg  edge) ) 
(cond  ((null  crossing-pt) 

(setf  prel  (i-cost  from 
(first  edge) 
current) ) 

(setf  postl  (i-cost  (first  edge) 
to 

(segment-next-win 
current) ) ) 

(setf  pre2  (i-cost  from 
(second  edge) 
current) ) 

(setf  post2  (i-cost  (second  edge) 
to 

( segment -next -win 
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current ) ) ) 

(setf  cl  (+  prel  postl) ) 

(setf  c2  (+  pre2  post2) ) 

(if  (<  cl  c2) 

(append  (list  cl)  (list  prel) 

(list  postl)  (list  (first  edge))  (list  *  (i  i))) 

(append  (list  c2)  (list  pre2) 

(list  post2)  (list  (second  edge))  (list  ' (i  i) ) ) ) ) 
(t 

(setf  prel  (i-cost  from 
crossing-pt 
current ) ) 

(setf  postl  (i-cost  crossing-pt 
to 

(segment-next -win 
current ) ) ) 

(setf  cl  (+  prel  postl) ) 

(append  (list  cl)  (list  prel) 

(list  postl)  (list  crossing-pt)  (list  ' (i  i) )))))) 

» 

;  isotropic-to-power  search 

(defun  ip-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(new-pt  (segment -exit-point  current)) 

(cl)  (c2)  (c3)  (c4)  (c-min) ) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi) ) 

(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy) ) ) ) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy)))) 

(setf  prel  (i-cost  from  pi  current)) 

(setf  postl  (p-cost  pi  to  (segment-next-win  current))) 

(setf  pre2  (i-cost  from  p2  current) ) 

(setf  post2  (p-cost  p2  to  (segment-next-win  current))) 

(setf  pre3  (i-cost  from  p3  current) ) 

(setf  post3  (p-cost  p3  to  (segment-next-win  current))) 
(setf  pre4  (i-cost  from  p4  current)) 
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(setf  post4  (p-cost  p4  to  {segment -next-win  current))) 
(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post 3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3))) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (“  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

'(ip)))) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

'  (i  P) ))) 

( (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

’  (i  P)))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  ’ (i  p) )))))) 


/ 

;  isotropic-to-bra)cing  search 

(defun  ib-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  {p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1) ) 

(cl)  (c2)  (c3)  (c4)  (c-min)) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi) ) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 
(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 
(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy)))) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy) ) ) ) 

(setf  prel  (i-cost  from  pi  current)) 
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(setf  postl  (b-cost  pi  to 

(segment-frontier  current) 

(segment -frontier 

(segment -next-win  current)))) 

(setf  pre2  (i-cost  from  p2  current) ) 

(setf  post2  (b-cost  p2  to 

(segment-frontier  current) 
(segment-frontier 

(segment-next-win  current)))) 

(setf  pre3  (i-cost  from  p3  current)) 

(setf  post3  (b-cost  p3  to 

(segment-frontier  current) 

( segment-frontier 

(segment -next-win  current)))) 

(setf  pre4  (i-cost  from  p4  current)) 

(setf  post4  (b-cost  p4  to 

(segment-frontier  current) 

( segment-frontier 

(segment-next-win  current)))) 

(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4) ) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3) ) ) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

'  (i  b)))) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

Mi  b)))) 

( (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

Mi  b)))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  Mi  b) )))))) 


t 

;  power-to-isotropic  search 

(defun  pi-search  (from  edge  to  current  loop-flag) 
(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 
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(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min)) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge)) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy)))) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy) ) ) )  (setf  prel  (p-cost  from  pi 

current) ) 

(setf  postl  (i-cost  pi  to  (segment-next -win  current))) 

(setf  pre2  (p-cost  from  p2  current)) 

(setf  post2  (i-cost  p2  to  (segment-next -win  current) ) ) 

(setf  pre3  (p-cost  from  p3  current) ) 

(setf  post3  (i-cost  p3  to  (segment-next -win  current) ) ) 

(setf  pre4  (p-cost  from  p4  current) ) 

(setf  post4  (i-cost  p4  to  (segment-next-win  current) ) ) 

(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3))) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

( second  edge ) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

' (p  i)))) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

' (P  i)))) 

( (“  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

' (p  i)))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  • (P  i) )))))) 

t 

;  power-to-power  search 
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(defun  pp-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min) ) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy)))) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy))))  (setf  prel  (p-cost  from 

pi  current)) 

(setf  postl  (p-cost  pi  to  (segment-next -win  current))) 

(setf  pre2  (p-cost  from  p2  current) ) 

(setf  post2  (p-cost  p2  to  (segment-next-win  current))) 

(setf  pre3  (p-cost  from  p3  current)) 

(setf  post3  (p-cost  p3  to  (segment-next-win  current))) 
(setf  pre4  (p-cost  from  p4  current)) 

(setf  post4  (p-cost  p4  to  (segment -next-win  current))) 

(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2) ) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3) ) ) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 


(append 

(list 

cl) 

(list 

prel) 

(list 

postl) 

(list 

Pl) 

(list 

•  (P  p)))) 

( (=  c-min 
(append 

c2) 

(list 

c2) 

(list 

pre2) 

(list 

post2) 

(list 

P2) 

(list 

'  (P  P)))) 

( (=  c-min 
(append 

c3) 

(list 

c3) 

(list 

pre3) 

(list 

post3) 

(list 

P3) 

(list 

'  (p  p)))) 
(t 
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(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  ’ (P  P) )))))) 


/ 

;  power-to-braking  search 

(defun  pb-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min)) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 
(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 
(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy) ) ) ) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy) ) ) ) 

(setf  prel  (p-cost  from  pi  current) ) 

(setf  postl  (b-cost  pi  to 

(segment -frontier  current) 
( segment-frontier 

(segment-next-win  current)))) 
(setf  pre2  (p-cost  from  p2  current)) 

(setf  post2  (b-cost  p2  to 

(segment-frontier  current) 
(segment-frontier 

(segment-next-win  current)))) 
(setf  pre3  (p-cost  from  p3  current)) 

(setf  post3  (b-cost  p3  to 

(segment-frontier  current) 

( segment-frontier 

(segment-next-win  current)))) 
(setf  pre4  (p-cost  from  p4  current) ) 

(setf  post4  (b-cost  p4  to 

(segment-frontier  current) 

( segment-frontier 

(segment-next-win  current)))) 
(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 
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(setf  edge  (cons  pi  (list  p3))) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

'  (P  b) ) ) ) 

( (“  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

• (P  b) ))) 

( (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

’ (p  b)))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  • (P  b))))))) 

/ 

;  braking-to-isotropic  search 

(defun  bi-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min)) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (f round  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy) ) ) ) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy)))) 

(setf  prel  (b-cost  from  pi 

( segment-frontier 

(segment -prior -win  current) ) 
(segment-frontier  current))) 

(setf  postl  (i-cost  pi  to  (segment-next-win  current) ) ) 

(setf  pre2  (b-cost  from  p2 

(segment-frontier 

(segment -prior-win  current) ) 
(segment-frontier  current) ) ) 

(setf  post2  (i-cost  p2  to  (segment-next-win  current) ) ) 
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(setf  pre3  (b-cost  from  p3 

(segment-frontier  (segment -prior -win 

current) ) 

(segment -frontier  current) ) ) 

(setf  post 3  (i-cost  p3  to  (segment -next-win  current) ) ) 

(setf  pre4  (b-cost  from  p4 

(segment-frontier  (segment -prior-win 

current) ) 

(segment-frontier  current))) 

(setf  post4  (i-cost  p4  to  (segment -next-win  current) ) ) 

(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3))) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

' (b  i) ))) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

' (b  i)))) 

( (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

' (b  i)))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  '  (b  i) )) )  ))  ) 


/ 

;  braking-to-power  search 

(defun  bp-search  (from  edge  to  current  loop-flag) 
(let*  ((xl)  (yl)  (x2)  (y2) 

(pi)  (p2)  (p3)  (p4) 

(dx)  (dy) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 
(edge-length  (+  *edge-tolerance*  1)) 
(new-pt  (segment-exit-point  current)) 

(cl)  (c2)  (c3)  (c4)  (c-min)) 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge) ) 

(setf  xl  (first  pi)) 

(setf  yl  (second  pi)) 
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(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  *interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy)))) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy)))) 

(setf  prel  (b-cost  from  pi 

(segment-frontier 

(segment-prior-win  current) ) 
(segment-frontier  current))) 

(setf  postl  (p-cost  pi  to  (segment-next-win  current))) 
(setf  pre2  (b-cost  from  p2 

( segment-frontier 

(segment -prior-win  current) ) 

(segment -frontier  current))) 

(setf  post2  (p-cost  p2  to  (segment -next-win  current) ) ) 
(setf  pre3  (b-cost  from  p3 

( segment-frontier 

(segment -prior-win  current) ) 

(segment -frontier  current) ) ) 

(setf  post3  (p-  cost  p3  to  (segment-next-win  current) ) ) 
(setf  pre4  (b-cost  from  p4 

(segment-frontier 

(segment-prior-win  current) ) 
(segment-frontier  current))) 

(setf  post4  (p-cost  p4  to  (segment-next-win  current) ) ) 
(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2)) 

(setf  c3  (+  pre3  post3) ) 

(setf  c4  (+  pre4  post4)) 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3) ) ) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge) ) ) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 

(cond  ( (=  c-min  cl) 


(append 

'(bp)))) 

( («  c-min 

(list 

c2) 

cl) 

(list  prel) 

(list 

postl) 

(list 

Pl) 

(list 

(append 

’ (b  p) ))) 

( («  c-min 

(list 

c3) 

c2) 

(list  pre2) 

(list 

post2) 

(list 

p2) 

(list 

(append 

'(bp)))) 

(list 

c3) 

(list  pre3) 

(list  post3) 

(list 

P3) 

(list 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  • (b  p))))))) 
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;  braking-to-braking  search 

(defun  bb-search  (from  edge  to  current  loop-flag) 

(let*  ((xl)  (yl)  (x2)  (y2) 

(pi  (first  (segment-frontier  current) ) ) 

(p4  (second  (segment -frontier  current) ) ) 

(p2)  (p3) 

(dx)  (dy) 

(path-leg) 

(distl)  (dist2) 

(prel)  (pre2)  (pre3)  (pre4) 

(postl)  (post2)  (post3)  (post4) 

(edge-length  (+  *edge-tolerance*  1)) 

(cl)  (c2)  (c3)  (c4)  (c-min) ) 

(cond  ((equal  (third  pi) 

(third  p4) ) 

(setf  path-leg  (cons  from  (list  to))) 

(setf  pi  (find-int  path-leg  edge)) 

(cond  ((null  pi) 

(setf  distl  (+  (i-cost  from 
(first  edge) 
current) 

(i-cost  (first  edge) 
to 

( segment -next -win 
current) ) ) ) 

(setf  dist2  (+  (i-cost  from 
(second  edge) 
current) 

(i-cost  (second  edge) 
to 

( segment-next -win 
current) ) ) ) 

(if  (<  distl  dist2) 

(setf  pi  (first  edge) ) 

(setf  pi  (second  edgs) ) ) ) ) 

(setf  prel  (b-cost  from  pi 

(segment-frontier 
(segment -prior-win  current)) 
(segment-frontier  current))) 
(setf  postl  (b-cost  pi  to 

(segment-frontier  current) 
(segment -frontier 
(segment-next-win  current)))) 
(setf  cl  (+  prel  postl)) 

(append  (list  cl)  (list  prel) 

(list  postl)  (list  pi)  (list  '  (b  b) ) ) ) 

(t 

(loop 

(setf  pi  (first  edge)) 

(setf  p4  (second  edge)) 

(setf  xl  (first  pi) ) 
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(setf  yl  (second  pi)) 

(setf  x2  (first  p4)) 

(setf  y2  (second  p4)) 

(setf  dx  (fround  (/  (-  x2  xl)  ^interval*) ) ) 

(setf  dy  (fround  (/  (-  y2  yl)  *interval*) ) ) 

(setf  p2  (cons  (+  xl  dx) 

(list  (+  yl  dy) ) ) ) 

(setf  p3  (cons  (-  x2  dx) 

(list  (-  y2  dy) ) ) ) 

(setf  prel  (b-cost  from  pi 

( segment-f rontier  (segment -prior -win 

current) ) 

(segment-frontier  current))) 

(setf  postl  (b-cost  pi  to 

(segment-frontier  current) 

(segment-frontier  (seg:  ent-next-win 

current) ) ) ) 

(setf  pre2  (b-cost  from  p2 

(segment-frontier  (segment -prior -win 

current) ) 

(segment-frontier  current))) 

(setf  post2  (b-cost  p2  to 

(segment-frontier  current) 

( segment-f rontier  ( segment -next -win 

current) ) ) ) 

(setf  pre3  (b-cost  from  p3 

(segment -frontier  ( segment -prior -win 

current) ) 

(segment-frontier  current))) 

(setf  post3  (b-cost  p3  to 

(segment-frontier  current) 

(segment-frontier  (segment -next -win 

current) ) ) ) 

(setf  pre4  (b-cost  from  p4 

(segment-f ront ier  ( segment -prior -win 

current) ) 

(segment-frontier  current))) 

(setf  post4  (b-cost  p4  to 

(segment-frontier  current) 

( segment-frontier  ( segment -next -win 

current) ) ) ) 

(setf  cl  (+  prel  postl)) 

(setf  c2  (+  pre2  post2) ) 

(setf  c3  (+  pre3  post3)) 

(setf  c4  (+  pre4  post4n 

(setf  c-min  (min  cl  c2  c3  c4)) 

(if  (<  c2  c3) 

(setf  edge  (cons  pi  (list  p3))) 

(setf  edge  (cons  p2  (list  p4)))) 

(setf  edge-length  (distance  (first  edge) 

(second  edge))) 

(if  (or  (null  loop-flag) 

(<  edge-length  *edge-tolerance*) ) 

(return) ) ) 
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(cond  ( (=  c-min  cl) 

(append  (list  cl)  (list  prel)  (list  postl)  (list  pi)  (list 

‘ (b  b)))) 

( (=  c-min  c2) 

(append  (list  c2)  (list  pre2)  (list  post2)  (list  p2)  (list 

’ (b  b)))) 

(  (=  c-min  c3) 

(append  (list  c3)  (list  pre3)  (list  post3)  (list  p3)  (list 

’ (b  b) ))) 

(t 

(append  (list  c4)  (list  pre4)  (list  post4)  (list  p4) 
(list  • (b  b))))))))) 

0 

;  If  no  intersection,  assume 
;  heading  is  on  a  critical 
;  angle  and  return  mid-point 
;  of  segment  as  switch-point . 

(defun  switch-point  (pi  p2  hdgl  hdg2) 

(let  ( (rayl  (calc-ray  pi  hdgl)) 

(ray2  (calc-ray  p2  hdg2) ) 

(int-pt) ) 

(setf  int-pt  (line-int  rayl  ray2)) 

(if  (null  int-pt) 

(mid-point  pi  p2) 
int-pt)  )  ) 

•  ifk'k-kiritTit'k'k'kifkititit'kif'kiririt-k'k'kir-k'k'kidfifidt 

(defun  i-cost  (pi  p2  current) 

(w-distance  pi  p2  (segment -weight  current) ) ) 

(iletun  p-cost  (pi  p2  current) 

(let  (  (sw-point  (switch-point  pi  p2  (segm.ent-critica] -p  current) 

(+  pi  (segment-dual-p  cun- it))))) 

(+  (i-cost  pi  sw-point  current) 

(i-cost  sw-poini  p2  current)))) 

;  the  function  sv.itch-point  is  in  "geometry .  ci" 


;  Note  b-cost  gets  different  arguments  than  i-cost,  p-cost 

(defun  b-cost  (pi  p2  edgel  edge2) 

(let  ((elevl)  (elev2)) 

(cond  ( (third  pi ) 

(setf  elevl  (third  pi))) 

((equal  pi  *start*) 

(setf  elevl  (find-elev  pi  (first  *rlist*)))) 

(t 

(setf  elevl  (calc-elev  pi  edgel)))) 

(cond  (  (third  p2 ) 

(setf  e]ev2  (third  p2))) 

((equal  p2  *goal*) 
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(setf  elev2  (find-elev  p2  (first  (last  *rlist*))))) 
(t 

(setf  elev2  (calc-elev  p2  edge2)))) 

(abs  (-  elevl  elev2) ) ) ) 


(defun  hi-cost  (pi  p2  current) 

(if  (and  (equal  (first  pi)  (first  p2) ) 

(equal  (second  pi)  (second  p2))) 

0 

(+  (w-distance  pi  p2  (segment-weight  current)) 
*infinity*) ) ) 
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Filename;  drawf.cl 
By:  HILTON,  Cary  A. 

Date:  May  91 

Function:  Various  drawing  functions 

1.  draws  map,  path  segments,  heading  ranges 

2.  computes  segment  headings,  segment  costs 


(defvar  *bad-segments*  nil)  ;  list  of  path  segments 

;  with  impermissible  headings 


(defun  draw-regions  (rlist) 

(let  ((polygon)  (vlist) ) 

(dolist  (element  rlist) 

(setf  polygon  (eval  element)) 

(setf  vlist  (polygon-vlist  polygon) ) 
(draw-edge  (first  vlist) 

(first  (last  vlist))) 

(loop 

(if  (null  (rest  vlist) ) 

(return) ) 

(draw-edge  (first  vlist) 

(second  vlist) ) 

(setf  vlist  (rest  vlist)))))) 

(defun  draw-region  (region) 

(let*  ( (r  (eval  region) ) 

(vlist  (polygon-vlist  r) ) ) 
^draw-edge  (first  vlist) 

(first  (last  vlist) ) ) 

(loop 

(if  (null  (rest  vlist) ) 

(return) ) 

(draw-edge  (first  vlist) 

(second  vlist)) 

(setf  vlist  (rest  vlist))))) 


(defun  draw-edge  (xyzl  xyz2) 

(let  ((pi  (eval  xyzl)) 

(p2  (eval  xyz2) ) ) 

(draw-line-xy  *map*  (first  pi)  (second  pi) 
(first  p2)  (second  p2) 

: width  2) ) ) 

(defun  draw-dotted  (edge) 
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(let  ((pi  (first  edge)) 

(p2  (second  edge))) 

(draw-line-xy  *map*  (first  pi)  (second  pi) 
(first  p2)  (second  p2) 

: dashing  2 
: width  2) ) ) 


(defun  draw-start  (s) 

(draw-f illed-circle-xy  *map*  (first  s) 

(second  s) 

3)) 


(defun  draw-goal  (g) 

(draw-circle-xy  *map*  (first  g) 

(second  g) 

3)) 


(defun  draw-path-dots  (current) 
(loop 

(cond  (  (null  current) 
(return) ) 

(t 


point 

point 


(draw-f illed-circle-xy  *map*  (first  (segment-exit 
current) ) 

(second  (segment-exit 

current) )  2) ) ) 

(setf  current  (segment-next-win  current) ) ) ) 


(defun  draw-path-lines  (current) 

(loop 

(cond  ((null  current) 

(return) ) 

(t 

(draw-line-xy  *map* 

(first  (segment -exit-point 

(segment-prior-win  current))) 
(second  (segment -exit -point 

(segment-prior-win  current))) 
(first  (segment -exit -point  current)) 

(second  (segment -exit -point  current)) 

: dashing  3) ) ) 

(setf  current  (segment-next-win  current) ) ) ) 


(defun  draw-final  (current) 

(let  ((path-cost  0)  (heading)) 

(setf  *bad- segments*  nil) 

(loop 

(cond  ( (null  current) 

(return) ) 

(t 

(unless  (»  (segment-cost  current)  0.0) 

;  (pprint  " - ”) 

;  (pprint  "region”) 

;  (pprint  (segment -region  current)) 

(setf  heading  (rad-to-deg  (angle 
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current) ) 


(cons  (segment -exit-point 

(segment -prior-win  current) ) 

(list 

(segment -exit-point  current)))))) 

(setf  (segment -heading  current)  heading) 

(pprint  "segment  heading") 

(pprint  heading) 

(pprint  "segment-cost") 

(pprint  (segment-cost  current)) 

(cond  ((illegal-heading  current) 

(cond  ( (>  (segment-cost  current) 

*infinity*) 

(setf  *bad-segments* 

(append  *bad- segments* 

(list  (segment -region  ourrent) ) ) ) ) ) 

(setf  path-cost 

(+  path-cost  (segment-cost  current) ) ) 

(draw-line-xy  *map*  (first  (segment-exit-point 
(segment -prior-win  current) ) ) 

(second  (segment-exit-point 
(segment -prior-win  current) ) ) 

(first  (segment-exit-point  current) ) 

(second  (segment-exit-point 


: dashing  3) 

(draw-ranges  (segment-exit -point 

(segment-prior-win  current))  current)))) 
(if  (null  (segment-next -win  current)) 
(draw-ranges  *goal*  current) ) 

(setf  current  (segment-next -win  current))) 
(pprint  "path-cost  =") 

(pprint  path-cost) 

(cond  ((null  *bad-segments*) 

(pprint  "valid  headings  for  all  segments")) 

(t 


(pprint  "impermissible  path  segments  at:") 
(pprint  *bad- segments*) ) ) ) ) 


(defun  draw-best -path  (path) 

(let*  ( (path-cost  (first  path) ) 
(path-segments  (rest  path) ) 

(prior-point  *start*) 

(current  (first  path-segments))) 

(clear  *map*) 

(draw-regions  *global-region-list*) 
(draw-filled-circle-xy  *map*  (first  *start*) 

(second  *start*) 

3) 

(loop 

(if  (null  current) 

(return) ) 

(unless  (=  (path-cost  current)  0.0) 

(pprint  " - ") 

(pprint  "region") 

(pprint  (path-region  current)) 
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(pprint  "segment  heading") 

(pprint  (path-heading  current) ) 

(pprint  "segment-cost") 

(pprint  (path-cost  current) ) 

(draw-line-xy  *map*  (first  (path-point  current)) 

(second  (path-point  current) ) 
(first  prior-point) 

(second  prior-point) 

: dashing  4) 

(draw-circle-xy  *map*  (first  (path-point  current) ) 

(second  (path-point  current) ) 

3) )  ;  end  unless 

(setf  prior-point  (path-point  current)) 

(setf  path-segments  (rest  path-segments) ) 

(setf  current  (first  path-segments)))  ;  end  loop 

(pprint  " - ") 

(pprint  "best  path  cost:") 

(pprint  path-cost) ) ) 

(defun  draw-direct  (pi  p2  current) 

(draw-line-xy  *map*  (first  pi)  (second  pi) 

(first  p2)  (second  p2) 

: dashing  3) 

(draw-ranges  pi  *first-win*) 

(draw-ranges  p2  *first-win*) ) 

(defun  draw-bounds  (element) 

(loop 

(unless  (or  (null  element) 

(=  (segment-cost  element)  0.0)) 

(draw-ranges  (segment-exit -point 

(segment-prior-win  element))  element) 

(if  (null  (segment-next-win  element)) 

(draw-ranges  (segment-exit-point 

(segment -prior -win  current) )  element) 

(setf  element  (segment -next -win  element) ) 

(if  (null  elemenL) 

(return) ) ) ) ) ) 

(defun  draw-circle  (current) 

(draw-circle-xy  *map*  (first  (segment-exit-point  current) ) 

(second  (segment-exit -point  current))  2)) 

(defun  draw-dot  (current) 

(draw-f illed-circle-xy  *map*  (first  (segment-exit-point 
current) ) 

(second  (segment-exit-point  current))  2)) 


(defun  draw-d  (point) 

(draw-f illed-circle-xy  *map*  (first  point) 

(second  point)  2)) 

(defun  whiteout  (current) 

(let  ((pi  (segment -exit -point  (segment -prior-win  current))) 
(p2  (segment-exit -point  current)) 

(p3  (segment -exit-point  (segment-next-win  current)))) 
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(setf  (window-stream-foreground-color  *niap*)  white) 
(draw-line-xy  *map*  (first  pi)  (second  pi) 

(first  p2)  (second  p2)) 

(draw-line-xy  *inap*  (first  p2)  (second  p2) 

(first  p3)  (second  p3) ) 

(setf  (window-stream-foreground-color  *map*)  blaclc) ) ) 

(defun  draw-current  (element) 

(unless  (null  (segment -prior-win  element)) 

(draw-line-xy  *map*  (first  (segment -exit-point  (segment -prior- 
win  element ) ) ) 

(second  (segment-exit-point  (segment -prior-win 

element) ) ) 

(first  (segment -exit-point  element)) 

(second  (segment -exit -point  element)) 

: dashing  4) ) 

(unless  (null  (segment -next-win  element)) 

(draw-line-xy  *map*  (first  (segment -exit -point  element) ) 

(second  (segment -exit -point  element) ) 

(first  (segment-exit-point  (segment-next-win 

element) ) ) 

(second  (segment-exit-point  (segment-next-win 

element) ) ) 

: dashing  4) ) ) 

(defun  draw-r  (pi  p2) 

(unless  (or  (null  pi) 

(null  (first  p2) ) ) 

(draw-line-xy  *map*  (first  pi)  (second  pi) 

(first  (first  p2) )  (second  (first  p2) ) ) ) ) 


(defun  qdraw  () 

(clear  *map*) 

(draw-start  *start*) 

(draw-goal  *goal*) 

(draw-regions  *global-region-list*) 

(draw-path-lines  *f irst-win*) ) 

(defun  draw-r  (pi  p2) 

(unless  (or  (null  pi) 

(null  (first  p2) ) ) 

(draw-line-xy  *map*  (first  pi)  (second  pi) 

(first  (first  p2))  (second  (first  p2))))) 
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;  Filename:  geometry. cl 
;  By:  HILTON,  Cary  A. 

;  Date:  May  91 

;  Function:  contains  geometric  spatial 
;  reasoning  functions 

;  1 .  includes  functions  to  determine 

segments. 


and  critical  angles  for  power. 


braking 


angles  of  line 
sideslope,  and 


limits 

2.  determines  if  a  vehicle  heading  is  inside 
a  non-permissible  heading  range 

3.  computes  line  intersections 


(setf  *ray-length*  920) 

(setf  *radius*  12) 

(setf  *radius2*  8) 

(setf  *offset*  (/  pi  2)) 

(defun  angle  (edge) 

(let  ((pi  (first  edge)) 

(p2  (second  edge) ) 

(offset  (/  pi  2) ) 

(angle) ) 

(if  (or  (eq  pi  nil)  (eq  p2  nil)) 

(setf  angle  nil) 

(setf  angle  (-  offset  (atan  (-  (second  p2)  (second  pi) ) 
(-  (first  p2)  (first  pi)))))) 

(mod  angle  *pi2*))) 

(defun  angle-between  (el  e2) 

(sin  (abs  (-  (angle  el)  (angle  e2))))) 

(defun  deg-to-rad  (degrees) 

(*  degrees  0.0174533)) 

(defun  rad-to-deg  (radians) 

(/  radians  0.0174533)) 

;  accepts  radian  measure 
(defun  heading-p~b  (slope  limit) 

(cond  ( (and  (>  slope  0) 

(>  slope  (abs  limit) ) ) 

(acos  (/  (tan  limit)  (tan  slope)))) 

(t  0))) 


;  extend  ray  to  intersect  edge 
;  radius  for  sideslope  range  icon 
;  radius  for  power  range  icon 
;  orient  north  to  0  degrees 
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;  accepts  radian  measure 
(defun  heading-s  (slope  limit) 

(cond  ( (and  (>  slope  0) 

(>  slope  limit) ) 

(asin  (/  (tan  limit)  (tan  slope)))) 

(t  0))) 

(defun  calc-ray  (point  angle) 

(let*  ( (x  (+  (*  *ray-length*  (sin  angle))  (first  point))) 
(y  (+  (*  *ray-length*  (cos  angle))  (second  point))) 
(end-pt  (cons  x  (list  y) ) ) ) 

(cons  point  (list  end-pt)))) 

(defun  find-region  (point) 

(let*  ( (region-list  *global-region-list*) 

(current-region  (first  region-list)) 

(inside) ) 

(loop 

(cond  ((null  region-list) 

(return) ) 

(t 

(setf  inside  (inside-region  point  current-region) ) 
(if  inside 
(return) ) 

(setf  region-list  (rest  region-list) ) 

(setf  current-region  (first  region-list))))) 
inside) ) 

(defun  betweenp  (a  b  x) 

(or  (and  (>=  x  a)  (<=  x  b) ) 

(and  (>=  X  b)  (<=  x  a)M) 

(defun  on-segment  (edge  point) 

(let  ( (xl  (first  (first  edge))) 

(yl  (second  (first  edge) ) ) 

(x2  (first  (second  edge))) 

(y2  (second  (second  edge) ) ) 

(xt  (first  point) ) 

(yt  (second  point))) 

(and  (and  (betweenp  xl  x2  xt) 

(betweenp  yl  y2  yt)) 

(zerop  (-  (*  (-  x2  xl)  (-  yt  yl) ) 

(*  (-  y2  yl)  (-  xt  xl))))))) 

(defun  inside-region  (point  region) 

(let*  ( (vlist  (pOlygon-vlist  (eval  region))) 

(ray  (calc-ray  point  pi) ) 

(edge  (cons  (eval  (first  (last  vlist))) 

(list  (eval  (first  vlist))))) 

(intercept  (find-int  ray  edge) ) 

(cross-count  0)) 

(unless  (null  intercept) 

(setf  intercept  (cons  (float  (first  intercept)) 

(list  (float  (second  intercept)))))) 

(if  (cross-p  ray  edge) 
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(incf  cross-count)) 

(if  (equal  intercept  (butlast  (first  edge)  1) ) 
(incf  cross-count)) 

(loop 

(setf  edge  (cons  (eval  (first  vlist)) 

(list  (eval  (second  vlist))))) 

(if  (cross-p  ray  edge) 

(incf  cross-count)) 

(if  (equal  intercept  (butlast  (first  edge)  1) ) 
(incf  cross-count) ) 

(setf  vlist  (rest  vlist)) 

(if  (null  (rest  vlist)) 

(return) ) ) 

(if  (oddp  cross-count) 
region 
nil))) 

;  accepts  radian  measure 
(defun  draw-ranges  (point  current) 

(let 

((p-xl)  (p-yl)  (p-x2)  (p-y2) 

(r-xl)  (r-yl)  (r-x2)  (r-y2) 

(l-x3)  (l-y3)  (l-x4)  (l-y4) 

(b-xl)  (b-yl)  (b-x2)  (b-y2) 

(slope  (segment-slope  current))) 


(unless  (null  (segment-critical-p  current) ) 


(setf 

p-xl 

(+  (*  *radius2* 

(sin 

(segment-critical-p 

current)))  (first  point))) 

(setf 

p-yl 

(+  (*  *radius2* 

(cos 

(segment-critical-p 

current)))  (second  point))) 

(setf  p-x2  (+  (*  *radius2*  (sin  (segment-dual-p  current))) 
(first  point) ) ) 

(setf  p-y2  (+  (*  *radius2*  (cos  (segment-dual-p  current))) 
(second  point) ) ) ) 


(unless  (null  (segment-critical-r  current) ) 

(setf  r-xl  (+  (*  *radius*  (sin  (segment-critical-r  current))) 
(first  point) ) ) 

(setf  r-yl  (+  (*  *radius*  (cos  (segment-critical-r  current))) 
(second  point))) 

(setf  r-x2  (+  (*  *radius*  (sin  (segment-dual-r  current))) 
(first  point) ) ) 

(setf  r-y2  (+  (*  *radius*  (cos  (segment-dual-r  current))) 
(second  point) ) ) ) 


(unless  (null  (segment-critical-1  current) ) 

(setf  l-x3  (+  (*  *radius*  (sin  (segment-critical-1 
(first  point) ) ) 

(setf  l-y3  (+  (*  *radius*  (cos  (segment-critical-1 
(second  point) ) ) 

(setf  l-x4  (+  (*  *radius*  (sin  (segment-dual-1 
(first  point) ) ) 

(setf  l-y4  (+  (*  *radius*  (cos  (segment-dual-1 
(second  point) ) ) ) 


current) ) ) 
current) ) ) 
current) ) ) 
current) ) ) 
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(unless  (null  (segment-critical-b  current) ) 

(setf  b-xl  (+  (*  *radius*  (sin  (segment-critical-b  current))) 
(first  point) ) ) 

(setf  b-yl  (+  (*  *radius*  (cos  (segment-critical-b  current))) 
(second  point) ) ) 

(setf  b-x2  (+  (*  *radius*  (sin  (segment-dual-b  current))) 
(first  point))) 

(setf  b-y2  (+  (*  *radius*  (cos  (segment-dual-b  current))) 
(second  point) ) ) ) 


(draw-circle-xy  *map* 

(first  point) 
(second  point) 
*radius* 


: dashing  5) 

(cond  ( (>  slope  0) 

(if  (>  slope  (vehicle-angle-p  ^vehicle*)) 

(draw-f illed-triangle-xy  *map* 

(first  point) 

(second  point) 
p-xl 
p-yl 
p-x2 
p-y2) 

nil) 

(cond  ( (>  slope  (vehicle-angle-s  *vehicle*)) 

(draw-f iJ led-triangle-xy  *map* 

(first  point) 

(second  point) 

r-xl 

r-yl 

r-x2 

r-y2) 

(draw-f illed-triangle-xy  *map* 

vfirst  point) 

(second  point) 

l-x3 

l-y3 

l-x4 

l-y4)) 

(t  nil) ) 

(if  (>  slope  (abs  (vehicle-angle-b  ^vehicle*))) 
(draw-triangle-xy  *map* 

(first  point) 

(second  point) 

b-xl 

b-yl 

b-x2 

b-y2 

;dashing  3) 


nil) ) 

(t  nil)))) 
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(defun  sign  (x) 

(cond  ( (plusp  x)  1) 

( (minusp  x)  -1) 

( (zerop  x)  0) ) ) 

(defun  area  (pi  p2  p3) 

(-  (*  (-  (first  p2)  (first  pi))  (-  (second  p3)  (second  pi))) 

(*  (-  (first  p3)  (first  pi))  (-  (second  p2)  (second  pi))))) 

(defun  slope-and-int  (pi  p2) 

(let  ( (xl  (first  pD) 

(yl  (second  pi) ) 

(x2  (first  p2) ) 

(y2  (second  p2) ) ) 

(cond  ( (*=  xl  x2)  (list  nil  xl) ) 

(t  (list  (/  (-  y2  yl)  (-  x2  xl) ) 

(-  y2  (*  x2  (/  (-  y2  yl)  (-  x2  xl)  )))))))  ) 

(defun  find-pt  (mbl  mb2) 

(let  ((ml  (first  mbl)) 

(bl  (second  mbl) ) 

(m2  (first  mb2) ) 

(b2  (second  mb2) ) ) 

(cond  ((equal  ml  m2) 

(cond  ( (not  (=  bl  b2) )  (list  nil  nil) ) 

(t  (list  nil  bl) ) ) ) 

((eq  ml  nil)  (list  bl  (+  b2  (*  m2  bl)))) 

( (eq  m2  nil)  (list  b2  (+  bl  (*  ml  b2)))) 

(t  (list  (fround  (/  (-  b2  bl)  (-  ml  m2))) 

(fround  (+  bl  (*  ml  (/  (-  b2  bl)  (-  ml  m2) )))))))) ) 

(defun  cross-p  (el  e2) 

(let  ((pi  (first  el)) 

(p2  (second  el) ) 

(p3  (first  e2) ) 

(p4  (second  e2) ) ) 

(if  (and  (/=  (sign  (area  pi  p2  p3)) 

(sign  (area  pi  p2  p4))) 

(/=  (sign  (area  p3  p4  pD) 

(sign  (area  p3  p4  p2)))) 

t 

nil))) 

;  Find  intersection  of  two  segments 

(defun  find-int  (el  e2) 

(let  ((mb-one)  (mb-two)  (int)) 

(cond  ((cross-p  el  e2) 

(setf  mb-one  (slope-and-int  (first  el)  (second  el))) 
(setf  mb-two  (slope-and-int  (first  e2)  (second  e2))) 

(setf  int  (find-pt  mb-one  mb-two))) 

(t  nil) ) ) ) 

;  Finds  intersection  of  two  lines 
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(defun  line-int  (el  e2) 

(let  ( (nib-one  (slope-and-int  (first  el)  (second  el) ) ) 

(mb-two  (slope-and-int  (first  e2)  (second  e2)))) 

(find-pt  mb-one  mb-two))) 

(defun  find-valid-crossing  (point  current) 

(let  ((ray-t)  (valid-point)) 

(setf  ray-t  (calc-ray  point  (segment -critical-r  current))) 

(setf  valid-point  (find-int  ray-t  (segment-frontier  current) ) ) 
(if  (null  valid-point) 

(setf  ray-t  (calc-ray  point  ( segment -dual-r  current))) 

(setf  valid-point  (find-int  ray-t  (segment-frontier 

current) ) ) ) 

(if  (null  valid-point) 

(setf  ray-t  (calc-ray  point  (segment -critical-1  current))) 
(setf  valid-point  (find-int  ray-t  (segment-frontier 

current) ) ) ) 

(if  (null  valid-point) 

(setf  ray-t  (calc-ray  point  (segment-dual-1  current))) 

(setf  valid-point  (find-int  ray-t  (segment-frontier 

current) ) ) ) 

valid-point) ) 


(defun  mar)c-pt  (int) 

(draw-f illed-circle-xy  *winl*  (first  int)  (second  int)  5) ) 

(defun  draw-line  (line) 

(draw-line-xy  *winl*  (first  (first  line))  (second  (first  line)) 
(first  (second  line))  (second  (second  line)) 

: brush-width  2)) 

(defun  find-elev  (point  current) 

(leL*  ((region  (eval  current)) 

(crossing-pts  (f ind-elev-points  point  region))) 

(calc-elev  point  crossing-pts))) 


(defun  f ind-elev-points  (point  current) 

(let  ( (vlist  (polygon-vlist  current)) 

(edge)  (inti)  (int2) 

(elevl)  (elev2) 

(rayl  (calc-ray  point  pi)) 

(ray2  (calc-ray  point  (*  pi  2)))) 

(setf  edge  (cons  (eval  (first  vlist)) 

(list  (eval  (first  (last  vlist)))))) 
(setf  inti  (find-int  rayl  edge)) 

(if  inti 

(setf  elevl  (calc-elev  inti  edge))) 

(setf  int2  (find-int  ray2  edge)) 

(if  int2 

(setf  elev2  (calc-elev  int2  edge))) 

(loop 

(setf  edge  (cons  (eval  (first  vlist)) 
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(list  (eval  (second  vlist))))) 

(cond  ((null  inti) 

(setf  inti  (find-int  rayl  edge)) 

(if  inti 

(setf  elevl  (calc-elev  inti  edge))))) 

(cond  ((null  int2) 

(setf  int2  (find-int  ray2  edge)) 

(if  int2 

(setf  elev2  (calc-elev  int2  edge))))) 

(setf  vlist  (rest  vlist)) 

(if  (or  (and  inti  int2) 

(null  (rest  vlist) ) ) 

(return) ) ) 

(setf  inti  (append  inti  (list  elevl) ) ) 

(setf  int2  (append  int2  (list  elev2) ) ) 

(cons  inti  (list  int2)))) 

(defun  calc-elev  (point  edge) 

(let*  ( (end-ptl  (first  edge) ) 

(end-pt2  (second  edge)) 

(edge-d)  (point-d) 

(low  (min  (third  end-ptl)  (third  end-pt2))) 

(elev-d  (abs  (-  (third  end-ptl) 

(third  end-pt2))))) 

(cond  ( (=  (first  end-ptl) 

(first  end-pt2)) 

(setf  edge-d  (abs  (-  (second  end-ptl) 

(second  end-pt2)))) 

(setf  point-d  (abs  (-  (second  end-ptl) 

(second  point) ) ) ) ) 

(t 

(setf  edge-d  (abs  (-  (first  end-ptl)  (first  end-pt2)))) 
(setf  point-d  (abs  (-  (first  end-ptl)  (first  point)))))) 
(if  (zerop  elev-d) 

(third  end-ptl) 

(+  (/  (*  point-d  elev-d)  edge-d)  low)))) 

(defun  calc-displ  (pi  p2) 

(let  (  (xl  (first  pi) ) 

(yl  (second  pi) ) 

(x2  (first  p2) ) 

(y2  (second  p2) ) ) 

(+  (abs  (-  xl  x2) ) 

(abs  (-  yl  y2) ) ) ) ) 

(defun  distance  (pi  p2) 

(let*  ( (xl  (first  pi)) 

(yl  (second  pi) ) 

(x2  (first  p2) ) 

(y2  (second  p2) ) 

(x  (-  x2  xl) ) 

(y  (-  y2  yl))) 

(sqrt  (+  (*  X  x)  (*  y  y))))) 

(defun  w-distance  (pi  p2  w) 
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(let*  ((xl  (first  pi)) 

(yl  (second  pi) ) 

(x2  (first  p2)) 

(y2  (second  p2) ) 

(X  (-  x2  xl)) 

(y  (-  y2  yl))) 

(*  (sqrt  (+  (*  X  x)  (*  y  y)))  w))) 

(defun  find-edge  (rl  r2) 

(let  (edge) 

(dolist  (element  (polygon-vlist  rl)  edge) 

(if  (memberp  element  (polygon-vlist  r2) ) 

(setf  edge  (append  edge  (list  element))))) 

(cond  ( (null  (rest  edge) ) 

(append  edge  edge) ) 

((and  (equal  (first  edge) 

(first  (polygon-vlist  rl))) 

(equal  (second  edge) 

(first  (last  (polygon-vlist  rl))))) 

(reverse  edge) ) 

(t 

edge) ) ) ) 

(defun  mid-point  (pi  p2) 

(let  ( (xl  (first  pi)) 

(yl  (second  pi) ) 

(x2  (first  p2)) 

(y2  (second  p2) ) ) 

(cond  ((or  (eq  nil  xl) 

(eq  nil  x2))  (list  nil  nil)) 

(t 

(list  (*  0.5  (+  xl  x2))  (*  0.5  (+  yl  y2))))))) 

(defun  illegal-heading  (current) 

(let  ((angle  (angle  (cons 

(segment -exit -point  (segment -prior-win  current) ) 
(list  (segment -exit -point  current))))) 

(cr  (segment-critical-r  current)) 

(dr  (segment-dual-r  current)) 

(cl  (segment-critical-1  current) ) 

(dl  (segment -dual-1  current) ) ) 

(if  (or  (inside-pl  angle  cl  dl) 

(inside-rb  angle  cr  dr) ) 
t 

nil) ) ) 

(defun  inside-pl  (angle  critical  dual) 

(cond  ( (>  critical  dual) 

(if  (and  (<  angle  (-  critical  .0174)) 

(>  angle  (+  dual  .0174))) 
t 

nil)) 

( (>  dual  critical) 

(if  (and  (>  angle  (-  critical  .0174)) 

(<  angle  (+  dual  .0174))) 
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nil 

t)) 

(t  nil))) 

(defun  inside-rb  (angle  critical  dual) 

(cond  ( (>  critical  dual) 

(if  (and  (>  angle  (-  dual  .0174)) 

(<  angle  (+  critical  .0174))) 

nil 

t)) 

( (>  dual  critical) 

(if  (and  (>  angle  (+  critical  .0174)) 

(<  angle  (-  dual  .0174))) 
t 

nil)) 

(t  nil))) 

(defun  virtual-obstacle  (current) 

(let*  ((region  (eval  current)) 

(ps-rad  (deg-to-rad  (polygon-slope  region) ) ) 

(vp-rad  (deg-to-rad  (vehicle-angle-p  *vehicle*))) 
(vs-rad  (deg-to-rad  (vehicle-angle-s  *vehicle*))) 
(critical-p  (heading-p-b  ps-rad  vp-rad) ) 

(critical-s  (heading-s  ps-rad  vs-rad) ) ) 

(if  (<  critical-p  critical-s) 
t 

nil)  ) ) 

(defun  valid-entry  (edge-from  edge-to  region) 

(let*  ( (r  (eval  region)) 

(pfl)  (pf2)  (ptl)  (pt2) 

(rotation) 

(slope  (deg-to-rad  (polygon-slope  r))) 

(cr)  (dr)  (cl)  (dl) 

(hi)  (h2)  (hdgl)  (hdg2)) 

(cond  ( (>  slope 

(vehicle-angle-s  *vehicle*) ) 

(setf  pfl  (first  edge-from)) 

(setf  pf2  (second  edge-from)) 

(setf  ptl  (first  edge-to)) 

(setf  pt2  (second  edge-to)) 

(setf  rotation  (deg-to-rad  (polygon-orientation  r) ) ) 
(setf  cr  (heading-s  slope 

(vehicle-angle-s  *vehicle*) ) ) 

(setf  dr  (mod  (+  (-  pi  cr)  rotation)  *pi2*) ) 

(setf  cl  (mod  (+  (-  cr)  rotation)  *pi2*)) 

(setf  dl  (mod  (+  (-  cr  pi)  rotation)  *pi2*) ) 

(setf  hi  (cons  pfl  (list  pt2) ) ) 

(setf  h2  (cons  pf2  (list  ptl))) 

(setf  hdgl  (angle  hi)) 

(setf  hdg2  (angle  h2)) 

(setf  cr  (+  cr  rotation)) 

(if  (or  (and  (inside-rb  hdgl  cr  dr) 

(inside-rb  hdg2  cr  dr) ) 

(and  (inside-pl  hdgl  cl  dl) 
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(inside-pl  hdg2  cl  dl) ) ) 

nil 

t)) 

(t 

t)))) 
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Filename:  links2.cl 
By:  HILTON,  Cary  A. 

Date:  May  91 

Function:  Processes  region  list  into  window  list 

1.  converts  list  into  doubly  linked 
circular  list 

2 .  creates  ' segment '  structure  for  use  in 
evaluating  window  sequences. 

3.  computes  critical  headings, 
initial  crossing  points, 

edge  common  to  adjacent  regions 


(setf  *init*  0)  ;  initialize  edge  count:  odd  points  to 

left,  even  points  to  right 

(setf  *print-circle*  1)  ;  turn  off  print  loop  for  circular  data 

structures  (doubly  linked  lists) 

(defconstant  *pi2*  (*  pi  2) ) 

/ 

(def struct  segment 
index 
region 
exit -point 
heading 
prior -point 
displ 
frontier 
frontier-length 
weight 
cost 

next-win 

prior-win 

critical-p 

dual-p 

critical-r 

dual-r 

crit-*  cal-1 

dual-1 

critical-b 

dual-b 

slope 

orientation) 

(defun  memberp  (item  input-list) 
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(if  input-list 
(if  (eq  item 

(first  input-list)) 
t 

(meinberp  item  (rest  input-list))))) 


(defun  double-lin)c  (head  tail) 

(unless  (null  tail) 

(setf  (segment-next-win  head)  (first  tail)) 
(setf  (segment -prior-win  (first  tail) )  head) 
(double-link  (first  tail)  (rest  tail) ) ) ) 


(defun  change-struct  (start  goal  rlist) 
(let*  ((win-t)  (weight-t) 

(exit-t)  (aft-t)  (next-t) 

(verts)  (edge-t)  (hdg-t)  (ray-t) 

(r)  (r-next) 

(ptl)  (pt2)  (ecount  0) 

(pwr-rad)  (side-rad)  (brk-rad) 
(crit-p)  (crit-r)  (crit-1)  (crit-b) 
(cp)  (dp)  (or)  (dr) 

(cl)  (dl)  (cb)  (db) 

(slope-rad)  (oriented) ) 

(if  rlist 

(setf  win-t  (list  (make-segment 

: region 


(setf  win-t  (list  (make-segment  : index  0 

rregion  (first  rlist) 

: exit-point  start 

: frontier  (cons 
start 

(list  start) ) ) ) ) ) 

(loop 

(cond  ( (null  rlist) 

(return) ) 

(t 

(setf  r  (eval  (first  rlist))) 

(setf  r-next  (eval  (second  rlist) ) ) 

(incf  ecount) 

/nd  (r-next 

(setf  verts  (find-edge  r  r-next) ) 

(setf  ptl  (eval  (first  verts) ) ) 

(setf  pt2  (eval  (second  verts)))) 

(t 

(setf  ptl  goal) 

(setf  pt2  goal) ) ) ) ) 

(setf  edge-t  (cons  ptl  (list  pt2))) 

(setf  weight-t  (polygon-weight  r) ) 

(setf  slope-rad  (deg-to-rad  (polygon-slope  r) ) ) 

(setf  oriented  (deg-to-rad  (polygon-orientation  r) ) 
(setf  pwr-rad  (vehicle-angle-p  *vehicle*)) 

(setf  side-rad  (vehicle-angle-s  *vehicle*)) 

(setf  brk-rad  (vehicle-angle-b  *vehicle*)) 

(setf  crit-p  (heading-p-b  slope-rad  pwr-rad)) 

(setf  cn’t-r  (heading-s  slope  rad  side-rad)) 

(setf  crit-1  (-  crit-r)) 
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(setf  crit-b  (heading-p-b  slope-rad  brk-rad) ) 

(setf  cp  (mod  (+  crit-p  oriented)  *pi2*) ) 

(setf  dp  (mod  (+  (-  crit-p)  oriented)  *pi2*) ) 

(setf  cr  (mod  (+  crit-r  oriented)  *pi2*) ) 

(if  (*  crit-r  crit-1) 

(setf  dr  (mod  (+  crit-r  oriented)  *pi2*)) 

(setf  dr  (mod  (+  (-  pi  crit-r)  oriented)  *pi2*) ) ) 

(setf  cl  (mod  (+  crit-1  oriented)  *pi2*)) 

(if  (“  crit-r  crit-1) 

(setf  dl  (mod  (+  crit-1  oriented)  *pi2*) ) 

(setf  dl  (mod  (+  (-  crit-r  pi)  oriented)  *pi2*))) 

(setf  cb  (mod  (+  crit-b  oriented)  *pi2*)) 

(setf  db  (mod  (+  (-  crit-b)  oriented)  *pi2*)) 

(cond  ( (inside-rb  cb  cr  dr) 

(setf  cb  dr) 

(setf  db  dl))) 

;  (cond  ( (oddp  ecount)  ;  set  point  at 

frontier  endpoint 

;  (setf  exit-t  (first  edge-t))) 

;  ( (evenp  ecount) 

;  (setf  exit-t  (second  edge-t)))) 

(setf  exit-t  (mid-point  ptl  pt2)  ;  set  point  at 

;  frontier 


midpoint 


(setf  temp  (make-segment  : index  e.^ount 


:  region 

(first  rlist) 

: exit -point 

exit-t 

:prior-point  exit-t 

:displ 

(+  *point-tolerance 

: frontier  edge-t 

: frontier-length 
(distance  (first  edge 

(second  edge-t) ) 

: weight 

weight -t 

:cost  nil 

: next -win 

nil 

:prior-win 

nil 

: slope 

slope-rad 

: orientation 

oriented 

:critical-p 

cp 

:dual-p 

dp 

:critical-r 

cr 

:dual-r 

dr 

:critical-l 

cl 

:dual-l 

dl 

:critical-b 

cb 

:dual-b 

db)) 

(setf  win-t  (append  win-t 
(setf  rlist  (rest  rlist) ) 

(if  (null  rlist) 

(list  temp) ) ) 

(return  win-t) ) ) ) ) 
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Filename:  partition. cl 
By:  HILTON,  Cary  A. 

Date:  May  91 

Function:  These  functions  partition  the 
current  window  edge  at  the 
points  where  the  cost  function 
changes . 

1.  The  possible  cost  functions  are: 
pp,  pi,  pb,  ip,  ii,  ib 

bp,  bu,  bb. 

"p"  =  power  range 

"i”  •=  isotropic  (unrestricted) 

"b"  =  braking  range 

2 .  performs  golden  ratio 

search  with  appropriate  cost  functions . 


(defvar  *infinity*  65536)  ;  represents  cost  of  "infinity" 

(defvar  *proot*)  ;  root  for  list  of  transition 

;  points  along  a  frontier 

(defstruct  partition 
displ 
t-point 
frontier 
range 
next 
prior) 


(defun  insert-part  (new  root) 

(let  ((top  root)  (current  root)) 

(cond  ( (null  current) 

(setf  current  new)) 

( (<  (partition-displ  new)  (partition-displ  current)) 
(setf  (partition-prior  current)  new) 

(setf  (partition-next  new)  current) 

(setf  current  new) ) 


(loop 
(if  (and 


current) ) 


current) ) 


(partition-displ 


(partition-displ 


(not  (null  (partition-next  current)))) 
(setf  current  (partition-next  current)) 
(return) ) ) 

(cond  ( (<  (partition-displ 


(partition-displ 


(setf  (partition-next  new)  current) 
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(setf  (partition-prior  new)  (partition-prior 

current) ) 

(setf  (partition-next  (partition-prior  current) ) 

new) 

(setf  (partition-prior  current)  new) 

(setf  current  top) ) 

(t 

(setf  (partition-next  new)  nil) 

(setf  (partition-next  current)  new) 

(setf  (partition-prior  new)  current) 

(setf  current  top))))))) 


(defun  insert -plist  (plist) 

(let  (root) 

(dolist  (element  plist  root) 

(setf  root  (insert-part  element  root))))) 

(defun  pre-part  (current) 

(let*  ((point  (segment-exit -point  (segment -prior-win  current))) 
(edge  (segment-frontier  current)) 

(rep)  (rdp)  (rcr)  (rdr) 

(rcl)  (rdl)  (rcb)  (rdb) 

(cp-int)  (dp-int)  (cr-int)  (dr-int) 

(cl-int)  (dl-int)  (cb-int)  (db-int) 

(plist) ) 

(cond  ((or  (eg  point  (first  edge)) 

(eg  point  (second  edge))) 
nil) 

(t 

(unless  (=  (segment-critical-p  current) 

(segment-dual-p  current)) 

(setf  rep  (calc-ray  point  (segment-critical-p  current))) 
(setf  rdp  (calc-ray  point  (segment-dual-p  current))) 

(setf  cp-int  (cons  (find-int  rep  edge)  '  (i) ) ) 

(setf  dp-int  (cons  (find-int  rdp  edge)  ' (p) ) ) ) 

(unless  (=  (segment-critical-r  current) 

(segment-dual-r  current)) 

(setf  rcr  (calc-ray  point  (segment-critical-r  current) ) ) 
(setf  rdr  (calc-ray  point  (segment-dual-r  current) ) ) 

(setf  rcl  (calc-ray  point  (segment-critical-1  current))) 
(setf  rdl  (calc-ray  point  (segment-dual-1  current) ) ) 

(setf  cr-int  (cons  (find-int  rcr  edge)  '(s))) 

(setf  dr-int  (cons  (find-int  rdr  edge)  ' (i) ) ) 

(setf  cl-int  (cons  (find-int  rcl  edge)  '(i))) 

(setf  dl-int  (cons  (find-int  rdl  edge)  '(s)))) 

(unless  (=  (segment-critical-b  current) 

(segment-dual-b  current)) 

(setf  rcb  (calc-ray  point  (segment-critical-b  current))) 
(setf  rdb  (calc-ray  point  (segment-dual-b  current))) 

(setf  cb-int  (cons  (find-int  rcb  edge)  ' (b) ) ) 

(setf  db-int  (cons  (find-int  rdb  edge)  '(i)))) 

(append  (list  dp-int)  (list  cr-int)  (list  dr-int)  (list  cb-int) 
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(list  db-int)  (list  dl-int)  (list  cl-int)  (list  dp- 

int)))  ))) 

(defun  post-part  (current) 

(let*  ((point  (segment -exit -point  (segment-next-win  current))) 
(edge  (segment-frontier  current)) 

(rep)  (rdp)  (rcr)  (rdr) 

(rcl)  (rdl)  (rcb)  (rdb) 

(cp-int)  (dp-int)  (cr-int)  (dr-int) 

(cl-int)  (dl-int)  (cb-int)  (db-int) 

(plist) ) 

(cond  ( (or  (eq  point  (first  edge) ) 

(eq  point  (second  edge))) 
nil) 

(t 

(unless  (=  (segment-critical-p  (segment-next-win  current)) 

( segment -dual-p  (segment -next-win  current))) 

(setf  rep  (calc-ray  point  (+  pi  (segment-critical-p 

(segment -next -win  current))))) 

(setf  rdp  (calc-ray  point  (+  pi  (segment-dual-p 

(segment-next -win  current))))) 

(setf  cp-int  (cons  (find-int  rep  edge)  * (p) ) ) 

(setf  dp-int  (cons  (find-int  rdp  edge)  '(i)))) 

(unless  (=  (segment-critical-r  (segment-next -win  current)) 

( segment -dual-r  (segment -next -win  current) ) ) 

(setf  rcr  (calc-ray  point  (+  pi  (segment-critical-r 

(segment-next-win  current))))) 

(setf  rdr  (calc-ray  point  (+  pi  (segment-dual-r 

(segment -next -win  current) ) ) ) ) 

(setf  rcl  (calc-ray  point  (+  pi  (segment-critical-1 

(segment-next -win  current))))) 

(setf  rdl  (calc-ray  point  (+  pi  (segment-dual-1 

(segraent-next-win  current))))) 

(setf  cr-int  (cons  (find-int  rcr  edge)  ' (i) ) ) 

(setf  dr-int  (cons  (find-int  rdr  edge)  '(s))) 

(setf  cl-int  (cons  (find-int  rcl  edge)  '  (s) ) ) 

(setf  dl-int  (cons  (find-int  rdl  edge)  ' (i) ) ) ) 

(unless  (=  (segment-critical-b  (segment-next -win  current)) 

( segment -dual-b  (segment -next-win  current) ) ) 

(setf  rcb  (calc-ray  point  (+  pi  (segment-critical-b 

(segment -next -win  current) ) ) ) ) 

(setf  rdb  (calc-ray  point  (+  pi  (segment -dual-b 

(segment -next-win  current))))) 

(setf  cb-int  (cons  (find-int  rcb  edge)  ' (i) ) ) 

(setf  db-int  (cons  (find-int  rdb  edge)  ' (b) ) ) ) 

(append  (list  cp-int)  (list  cr-int)  (list  dr-int)  (list  cb-int) 
(list  db-int)  (list  dl-int)  (list  cl-int)  (list  dp- 

int)))))) 

(defun  part  (rl  r2) 

(let*  ( (frontier  (segment-frontier  rl) ) 

(pre-point 

(segment-exit-point  (segment-prior-win  rl))) 

(t-point 
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(first  (segment-frontier  rl))) 

(post -point 

(segment-exit-point  r2)) 

(pre-trans-pts  (pre-part  rl)) 

(post-trans-pts  (post-part  rl) ) 

(init-range) 

(t-list) 

(segment-t) ) 

(setf  t-list 

(dolist  (element  pre-trans-pts  t-list) 

(unless  (null  (first  element)) 

(setf  temp  (ma)ce-partition 

:displ  (calc-displ  (first  element)  t 

point) 

:t-point  (first  element) 

: frontier  *pre 

; range  (second  element))) 

(setf  t-list  (append  t-list  (list  temp)))))) 

(setf  t-list 

(dolist  (element  post-trans-pts  t-list) 

(unless  (null  (first  element) ) 

(setf  temp  (ma)ce-partition 

rdispl  (calc-displ  (first  element)  t 

point) 

:t-point  (first  element) 

: frontier  'post 
: range  (second  element))) 

(setf  t-list  (append  t-list  (list  temp)))))) 
(setf  tenp  (make-partition 

:displ  *infinity* 

:t-point  (second  frontier) 

: frontier  nil 
: range  nil)) 

(setf  t-list  (append  t-list  (list  temp))) 

(setf  *proot*  (insert-plist  t-list)))) 


(defun  adjust-crossing  (current) 

(let*  ( (rl  current)  (r2  (segment -next -win  current)) 
(pre-point 

(segment-exit-point  (segment -prior-win  rl))) 
(t-point  (first  (segment-frontier  rl))) 
(post-point  (segment-exit-point  r2)) 

(pre-segment  'cons  pre-point  (list  t-point))) 
(post-segment  (cons  t-point  (list  post-point))) 
(best-frontier)  (frontier-t)  (best-range) 
(end-ptl)  (end-pt2)  (ray-t) 

(pre-range)  (post-range) 

(best-crossing) 

(next-crossing)  (loop-flag)) 

(setf  pre-range  (find-pre-range  pre-segment  rl)) 
(setf  post-range  (find-post-range  post-segment  r2)) 
(part  rl  r2) 

(loop 

(setf  end-ptl  t-point) 
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(setf  end-pt2  (partition-t-point  *proot*) ) 

(setf  frontier-t  (cons  end-ptl  (list  end-pt2))) 

(setf  next-crossing  (costf  pre-point  pre-range 

frontier-t 

post-point  post-range 
current 
nil)) 

(cond  ((or  (null  best-crossing) 

(<  (first  next-crossing)  (first  laest-crossing) ) ) 
(setf  best-crossing  next-crossing) 

(setf  best-frontier  frontier-t) 

(setf  best-range  (fifth  tiest-crossing) ) ) ) 

(if  (equal  (partition-frontier  *proot*)  ’pre) 

(setf  pre-range  (partition-range  *proot*) ) 

(setf  post-range  (partition-range  *proot*))) 

(setf  *proot*  (partition-next  *proot*) ) 

(if  (null  *proot*) 

(return) 

(setf  t-point  (second  frontier-t) ) ) )  ;  end  loop 

(setf  best-crossing  (costf  pre-point 

(first  best-range) 
best-frontier 
post -point 
(second  best-range) 
current 
t)) 


(cond  ( (<  (first  best-crossing) 
(+  (segment-cost  rl) 
(segment-cost  r2))) 


defun 


(setf  (segment -prior-point  rl)  (segment -exit-point  rl)) 
(whiteout  current) 

(draw-region  (segment-region  current)) 

(setf  (segment-cost  rl)  (second  best-crossing)) 

(setf  (segment-cost  r2)  (third  best-crossing) ) 

(setf  (segment-exit-point  rl)  (fourth  best-crossing)) 
(draw-current  current) 

(setf  (segment-displ  rl) 

(calc-displ  (segment-prior-point  rl) 

(segment-exit-point  rl) ))))))  ;  end  cond,  let 


(defun  print-plist  (current) 

(pprint  "plist") 

(loop 

(pprint  (partition-t-point  current)) 
(pprint  (partition-range  current)) 

(pprint” - ”) 

(setf  current  (partition-next  current) ) 
(if  (null  current) 

(return) ) ) ) 
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(defun  calc-pre-range  (heading  current) 

(cond  ( (inside-pl  heading 

(segment-critical-p  current) 

( segment -dual-p  current) ) 

•p) 

( (inside-rb  heading 

(segment-critical-r  current) 
(segment-dual-r  current)) 

•s) 

( (inside-pl  heading 

(segment-critical-l  current) 

(segment -dual-1  current)) 

•s) 

( (inside-rb  heading 

(segment-critical-b  current) 
(segment-dual-b  current)) 

•b) 

(t  'i))) 

(defun  calc-post-range  (heading  current) 

(cond  ( (inside-pl  heading 

(+  pi  (segment -dual-p  current) ) 

(+  pi  (segment-critical-p  current))) 
’P) 

(  (inside-rb  heading 

(+  pi  (segment-dual-r  current) ) 

(+  pi  (segment-critical-r  current))) 
’s) 

( (inside-pl  heading 

(+  pi  (segment -dual-1  current) ) 

(+  pi  (segment-critical-l  current))) 
•s) 

( (inside-rb  heading 

(+  pi  (segment-dual-b  cui-^nt)) 

(+  pi  (segment-critical-b  current))) 
■b) 

(t  •!))) 

(defun  find-post-range  (segment  region) 

(let  ( (hdg)  (range) ) 

(if  (eg  (first  segment) 

(second  segment)) 

(setf  hdg 

(angle  (segment-frontier 

(segment -prior-win  region) ) ) ) 
(setf  hdg  (angle  segment))) 

(setf  range  (calc-pre-range  hdg  region)))) 


(defun  find-pre-range  (segment  region) 
(let  ( (hdg)  (range) ) 

(if  (eq  (first  segment) 

(second  segment)) 

(setf  hdg 
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(angle  (segment -frontier  region))) 

(setf  hdg  (angle  segment))) 

(setf  range  (calc-pre-range  hdg  region)))) 
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;  Filename:  start. cl 
;  By:  HILTON,  Cary  A. 

;  Date:  May  91 

;  Function:  A*  search  returns  list  of  regions 
;  connecting  start  and  goal  points. 

;  1.  heuristic:  current  cost  +  weighted  distance  to 

goal 

;  2.  attempts  to  screen  path  segments  for 

;  impermissible  headings 

•  *****★★*■*★*★★**★★★★★*★*★★*■★*★★★★★★**★★*★***★★★**★★***★********★★ 
(defvar  *nroot*  nil) 

;  This  structure  forms  a  linked  list 
;  ordered  by  cost  which  determines 
;  the  next  node  for  expansion  during 

;  the  A*  region  search . 

(defstruct  node 
point 
edge 
chain 
region 
cost 

cost-est 

prior 

next) 

(defun  region-search  (start  goal 
start-region 
goal -region) 

(let*  ((active-region  start-region) 

(prior-edge  (cons  start  (list  start))) 

(current -edge) 

(active-node)  (active-node-cost) 

(est-to-goal)  (cost-tot  0)  (ten^j-node) 

(prior-node  start)  (prior-region)  (prior-cost  0) 

(back-path  (list  start-region))  (rlist) ) 

; (pprint  "start  point  is  in  region:") 

/ (pprint  *start*) (pprint  start-region) 

; (pprint  "goal  point  is  in  region:") 

; (pprint  *goal*) (pprint  goal-region) 

(setf  *nroot*  nil) 

(setf  current-node  (make-node 

: point  start 

;edge  nil 

: region  start-region 
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: chain 


nil 


0 


:cost 

;cost-est  0 

rprior  nil 

:next  nil)) 

; (pprint  "detour-region") (pprint  detour-region) 

(loop 

(dolist  (element  (polygon-alist  (eval  active-region) ) ) 

(unless  (or  (and  (eq  element  prior-region) 

(not  (eq  start -region  goal-region))) 
(virtual-obstacle  element) 

(and  (memberp  element  (node-chain  current - 

node) ) 

(not  (eq  start -region  goal-region) ) ) ) 

(setf  current-edge  (find-edge  (eval  active-region) 

(eval  element))) 

(setf  current-edge  (cons  (eval  (first  current-edge) ) 

(list 

(eval  (second  current-edge) ) ) ) ) 

(unless  (or  (null  (first  current-edge) ) 

(null  (valid-entry  prior-edge  current-edge 

active-region) ) ) 


(setf  active-node  (mid-point  (first  current-edge) 

(second  current-edge) ) ) 

(setf  active-node-cost 

(+  (w-distance  prior-node  active-node 
(polygon-weight  (eval  active-region) ) ) 
prior-cost) ) 


unless. 


(draw-dotted  (cons  active-node  (list  prior-node))) 
(setf  est-to-goal  (distance  active-node  goal)) 

(setf  cost-tot  (+  active-node-cost  est-to-goal) ) 
(setf  temp-node  (make-node 

; point  active-node 

:edge  current-edge 


: region 
: chain 

;cost 
:cost-est 
; prior 
inext 

(setf  *nroot*  (insert-node 
unless,  dolist 


element 

back-path 

active-node-cost 
cost-tot 
nil 
nil) ) 

temp-node  *nroot*))))) 


;end 


(if  (null  *nroot*) 

(return) 

(setf  current-node  *nroot*)) 

(setf  back-path  (append  (node-chain  current-node) 

(list  (node-region  current-node)))) 
(setf  prior-cost  (node-cost  current-node) ) 

(if  (eq  (node-region  current -node)  goal-region) 

(return) ) 
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(setf  prior-edge  (node-edge  current-node)) 

(setf  *nroot*  (node-next  *nroot*)) 

(setf  prior-region  active-region) 

(setf  prior-node  (node-point  current-node) ) 

(setf  active-region  (node-region  current-node))) 

(draw-dotted  (cons  (node-point  current -node) 

(list  goal))) 

(setf  rlist  (append  (node-chain  current-node) 

(list  (node-region  current-node) ) ) ) 

(if  (eq  (first  (last  rlist) )  goal-region) 
rlist 
nil) ) ) 

★  ★  ★  ★  ★ 

(defun  insert-node  (new  root) 

(let  ((top  root)  (current  root)) 

; (pprint  "inside  insert-node”) 

(cond  ( (null  current) 

(setf  current  new)) 

( (<  (node-cost-est  new)  (node-cost-est  current)) 

(setf  (node-prior  current)  new) 

(setf  (node-next  new)  current) 

(setf  current  new) ) 

(t  (loop 

(if  (and  (>=  (node-cost-est  new)  (node-cost-est 

current) ) 

(not  (null  (node-next  current)))) 

(setf  current  (node-next  current)) 

(return) ) ) 

(cond  ( (<  (node-cost-est  new)  (node-cost-est 

current) ) 

(setf  (node-next  new)  current) 

(setf  (node-prior  new)  (node-prior  current)) 

(setf  (node-next  (node-prior  current))  new) 

(setf  (node-prior  current)  new) 

(setf  current  top)) 

(t 

(setf  (node-next  new)  nil) 

(setf  (node-next  current)  new) 

(setf  (node-prior  new)  current) 

(setf  current  top))))))) 
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Filename:  vehicle. cl 
By:  HILTON,  Cary  A. 

Date :  May  91 
Function:  Vehicle  model. 

1.  vehicle  is  defined  by  its  power,  sideslope  and 
braking  limits . 

2 .  angles  are  measured  in  degrees  but  converted 
by  the  program  to  radian  measures . 

3.  default  is  vehicle-2 

4.  to  change  to  another  vehicle,  enter: 

(setf  ^vehicle*  (vehicle-rad  vehicle-x) ) 


(defstruct  vehicle 
angle-p 
angle-s 
angle-b) 


(defvar  vehicle-1  (make-vehicle  : angle-p  31.0 

; angle-s  17.0 
: angle-b  -10.0)) 


(defvar 

vehicle-2 

(make-vehicle 

: angle-p 

26.0 

: angle-s 

22.0 

: angle-b 

-5.0)) 

(defvar 

vehicle-3 

(make-vehicle 

; angle-p 

31.0 

: angle-s 

17.0 

: angle-b 

-5.0)) 

(defvar 

vehicle-4 

(make-vehicle 

: angle-p 

7.8 

:  angle-s  9.0 
:  angle-b  -8.0)) 


(defun  vehicle-rad  (vehicle) 

(make-vehicle  : angle-p  (deg-to-rad  (vehicle-angle-p  vehicle)) 
: angle-s  (deg-to-rad  (vehicle-angle-s  vehicle)) 
:angle-b  (deg-to-rad  (vehicle-angle-b  vehicle)))) 
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Filename :  map2 . cl 
By;  HILTON,  Cary  A. 

Date;  May  91 

Function;  Contains  map  data. 

1.  defines  vertices,  edges,  and  polygons  within 
the  map  boundaries 

2 .  specifies  which  vertices  and  edges  each  polygon 
is  made  from 

3.  contains  adjacency  list  for  polygons 
★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ 


;  This  map  closely  resembles  the  map  used 
;  by  Ron  Ross . 

•  ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ 
/ 


(def struct  edge 
ptl 
pt2) 

(defstruct  polygon 
vlist 
slope 

orientation 

weight 

alist) 

9 


;  Global  vertex  list 

(setf  *global-vertex-list*  ' (vl  v2  v3  v4  v5  v6  v7  v8  v9 

vlO  vll  vl2  vl3  vl4  vl5  vl6  vl7  vl8  vl9 

v20  v21  v22  v23  v24  v25  v26  v27  v28  v29 

v30  v31  v32  v33  v34  v35  v36  v37  v38  v39 

v40  v41  v42  v43  v44  v45) ) 

;  Global  polygon  list 

(setf  *global-region-list*  ' (rl  r2  r3  r4  r5  r6  r7  r8  r9 

rlO  rll  rl2  rl3  rl4  rl5  rl6  rl7  rl8  rl9 

r20  r21  r22  r23  r24  r25  r26  r27  r28  r29 

r30  r31  r32  r33  r34  r35  r36  r37  r38) ) 

(setf  gl  *global-region-list*) 
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(setf 

vl 

'  (352,0 

96.0 

14.5)) 

(setf 

v2 

'  (480.0 

224.0 

14.5)) 

(setf 

v3 

'  (400.0 

304.0 

14.5)) 

(setf 

v4 

•  (272.0 

176.0 

14.5) ) 

(setf 

v5 

'  (24C.0 

320.0 

9.0)) 

(setf 

v6 

’  (240.0 

448.0 

9.0)) 

(setf 

v7 

'  (112.0 

448.0 

9.0)) 

(setf 

v8 

'  (112.0 

320.0 

9.0)) 

(setf 

v9 

•  (192.0 

368.0 

27.75) ) 

(setf 

vlO 

'  (160.0 

368.0 

27.75) ) 

(setf 

vll 

'  (192.0 

400.0 

27.75) ) 

(setf 

vl2 

•  (160.0 

400.0 

27.75)) 

(setf 

vl3 

•  (352.0 

160.0 

35.75) ) 

(setf 

vl4 

'  (416.0 

224.0 

35.75) ) 

(setf 

vl5 

'  (400.0 

240.0 

35.75) ) 

(setf 

vl6 

'  (336.0 

176.0 

35.75) ) 

(setf 

vl7 

•  (352.0 

112.0 

18.0)) 

(setf 

vl8 

'  (464.0 

224,0 

18.0) ) 

(setf 

vl9 

'  (400.0 

288.0 

18.0)) 

(setf 

v20 

•  (288.0 

176.0 

18.0) ) 

(setf 

v21 

' (352.0 

48.0 

0.0)) 

(setf 

v22 

'  (528.0 

224.0 

0.0)  ) 

(setf 

v23 

•  (400.0 

352.0 

0.0)) 

(setf 

v24 

•  (224.0 

176.0 

0.0)) 

(setf 

v25 

'  (208.0 

352.0 

19,5) ) 

(setf 

v26 

'  (208.0 

416.0 

19.5)) 

(setf 

v27 

•  (144.0 

416.0 

19.5)) 

(setf 

v28 

'  (144.0 

352.0 

19.5)) 

(setf 

v29 

'  (256.0 

304.0 

9.0)) 

(setf 

v30 

'  (256.0 

464.0 

9.0)) 

(setf 

v31 

'  (  96.0 

464.0 

9.0)) 

(setf 

v32 

'  (  96.0 

304.0 

9.0)) 

(setf 

v33 

'  (288.0 

272.0 

0.0)  ) 

(setf 

v34 

'  (288.0 

496.0 

0.0)) 

(setf 

v35 

'  (  64.0 

496.0 

0.0)) 

(setf 

v36 

•  (  64.0 

272.0 

0.0)) 

(setf 

v37 

'  (  0.0 

790.0 

0.0)) 

(setf 

v38 

• (600.0 

790.0 

0.0)) 

(setf 

v39 

• (600.0 

496.0 

0.0)) 

(setf 

v40 

’  (600.0 

224.0 

0.0)) 

(setf 

v41 

'  (600.0 

0.0 

0.0)) 

(setf 

v42 

'  (352.0 

0.0 

0.0)) 

(setf 

v43 

'  (  64.0 

0.0 

0.0)) 

(setf 

v44 

•  (  0.0 

0.0 

0.0)) 
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(setf  v45  '(  0.0  496.0  0.0)) 


(setf  rl  (ina)ce-polygon  rvlist  '  (v3  v23  v22  v2) 

:  slope  23.1 

: orientation  225 
: weight  1 

:alist  ' (r32  r2  r7  r4))) 


(setf  r2  (ma)ce-polygon  rvlist  *  (vl9  v3  v2  vl8) 

: slope  17.2 

: orientation  225 
: weight  1 

ralist  ' (rl  r3  r8  r5))) 

(setf  r3  (make-polygon  rvlist  ' (vl5  vl9  vl8  vl4) 

r slope  27.6 

r orientation  225 
r weight  1 

ralist  • {rl3  r2  r6  r9))) 


(setf  r4  (make-polygon  rvlist  ' (v21  vl  v2  v22) 

: slope  23.1 

r orientation  315 
r weight  1 

ralist  ' (rl  r38  rlO  r5) ) ) 

(setf  r5  (make-polygon  rvlist  ' (vl  vl7  vl8  v2) 

r slope  17.2 

r orientation  315 
r weight  1 

ralist  ’ (r4  r6  r2  rll))) 

(setf  r6  (make-polygon  rvlist  ' (vl7  vl3  vl4  vl8) 

r slope  27.6 

r orientation  315 
r weight  1 

ralist  ' (r5  rl3  r3  rl2))) 

(setf  r7  (make-polygon  rvlist  ’ (v24  v23  v3  v4) 

r slope  23.1 

r orientation  135 
r weight  1 

ralist  ' (r34  r8  rl  rlO))) 

(setf  r8  (make-polygon  rvlist  ' (v4  v3  vl9  v20) 

r slope  17.2 

r orientation  135 
r weight  1 

ralist  ' (r7  r9  r2  rll))) 

(setf  r9  (make-polygon  rvlist  ’ (v20  vl9  vl5  vl6) 

r slope  27.6 
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:  orientation  135 
:  weight  1 

:alist  * (r8  rl3  r3  rl2))) 


(setf  rlO  (make-polygon  :vlist 

; slope  23.1 

: orientation  045 
: weight  1 

:alist  ' (r37 

(setf  rll  (make-polygon  :vlist 

: slope  17.2 

: orientation  045 
: weight  1 

ralist  ' (rlO 

(setf  rl2  (make-polygon  rvlist 

: slope  27.6 

: orientation  045 
: weight  1 

:alist  ' ^rll 

(setf  rl3  (make-polygon  :vlist 

: slope  0 . 0 

: orientation  0 
: weight  1 

:alist  ' (r9  r6  r3  rl2))) 


(setf 

rl4 

(make-polygon  rvlist 

’ (v36  v32  v29  v33) 

: slope 

15.7 

: orientation 

0 

: weight 

1 

ralist  ' 

(r35 

rl5  r26  rl8))) 

(setf 

rl5 

(make-polygon  rvlist 

’ (v32  v8  v5  v29) 

r  slope 

0.0 

r orientation 

0 

r weight 

1 

ralist  ’ (rl4 

rl6  rl9  r27))) 

(setf 

rl6 

(make-polygon  rvlist 

' (v8  v28  v25  v5) 

r  slope 

18.1 

r orientation 

0 

r  weight 

1 

ralist  ' 

(rl5 

rl7  r20  r28))) 

(setf 

rl7 

(make-polygon  rvlist 

' (v28  vlO  v9  v25) 

r slope 

27.3 

r orientation 

0 

r weight 

1 

ralist  ' 

(rl6 

r30  r21  r29))) 

(setf 

rl8 

(make-polygon  rvlist 

• (v35  v31  v32  v36) 

r slope 

15.7 

r orientation 

090 

' (v24  v4  vl  v21) 

rll  r7  r4))) 

’  (v4  v20  vl7  vl) 

rl2  r8  r5) ) ) 

• (v20  vl6  vl3  vl7) 

rl3  r9  r6) ) ) 

' (vl6  vl5  vl4  vl3) 
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: weight  1 

:alist  ' (r36  rl9  r22  rl4))) 

(setf  rl9  (make-polygon  :vlist  ' (v32  v31  v7  v8) 

: slope  0 . 0 

: orientation  090 

: weight  1 

:alist  ' (rl8  r20  r23  rl5))) 

(setf  r20  (make-polygon  :vlist  ' (v7  v27  v28  v8) 

: slope  18.1 

: orientation  090 

: weight  1 

:alist  • (rl9  r21  r24  rl6))) 


(setf  r21  (make-polygon  :vlist  ' (v27  vl2  vlO  v28) 

: slope  27.3 

: orientation  090 

; weight  1 

:alist  ' (r20  r30  r25  rl7))) 

(setf  r22  (make-polygon  :vlist  ' (v35  v34  v30  v31) 

: slope  15.7 

; orientation  180 

: weight  1 

:alist  '  (r31  r23  rl8  r26))) 

(setf  r23  (make-polygon  ;vlist  ' (v31  v30  v6  v7) 

: slope  0 . 0 

: orientation  180 

: weight  1 

:alist  ' (r22  r24  r27  rl9))) 

(setf  r24  (make-polygon  :vlist  ' (v27  v7  v6  v26) 

: slope  18.1 

: orientation  180 

: weight  1 

:alist  ' (r23  r25  r20  r28))) 

(setf  r25  (make-polygon  ;vlist  ' (vl2  v27  v26  vll) 

: slope  27.3 

: orientation  180 

: weight  1 

:alist  '(^24  r30  r21  r29) ) ) 

(setf  r26  (make-polygon  ivlist  ' (v30  v34  v33  v29) 

: slope  15.7 

••orientation  270 

: weight  1 

lalist  ' (r33  r27  r22  rl4))) 

(setf  r27  (make-polygon  :vlist  ' (v6  v30  v29  v5) 

: slope  0 

: orientation  270 
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: weight  1 

ralist  • (r26  r28  r23  rl5))) 

(setf  r28  (make-polygon  :vlist  ' (v26  v6  v5  v25) 

: slope  18.1 

: orientation  270 

: weight  1 

:alist  • (r27  r29  r24  rl6))) 

(setf  r29  (make-polygon  rvlist  ' (vll  v26  v25  v9) 

; slope  27.3  i 

; orientation  270 

; weight  1 

:alist  •(r28  r30  r25  rl7))) 

(setf  r30  (make-polygon  :vlist  ' (vlO  vl2  vll  v9) 

: slope  0 . 0 

: orientation  0 

: weight  1 

ralist  • (r21  r25  r29  rl7) ) ) 

(setf  r31  (make-polygon  rvlist  ' (v45  v37  v38  v39  v34  v35) 

.•slope  0.0 

r orientation  0 

r weight  1 

ralist  •  (r22  r36  r32) ) ) 

(setf  r32  (make-polygon  rvlist  ' (v34  v39  v40  v22  v23) 

r  slope  0 . 0 

r orientation  0 
r weight  1 

ralist  ' (r31  r33  rl  r38))) 

(setf  r33  (make-polygon  rvlist  ' (v34  v23  v33) 

r slope  0.0 

r orientation  0 
.•weight  1 

ralist  • (r26  r32  r34))) 

(setf  r34  (make-polygon  rvlist  ' (v24  v33  v23) 

r  slope  0 . 0 

r orientation  0 
r weight  1 

ralist  ’ (r7  r33  r35) ) ) 

(setf  r35  (make-polygon  rvlist  ' (v36  v33  v24)  ^ 

r  slope  0 . 0 

r orientation  0 

r weight  1  , 

ralist  '  (rl4  r34  r37))) 

(setf  r36  (make-polygon  rvlist  ' (v44  v45  v35  v36  v43) 

r slope  0.0 

r orientation  0 
r weight  1 
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:alist 


•  (r31  rl8  r37) ) ) 


(setf  r37  (make-polygon  rvlist  ' (v43  v36  v24  v21  v42) 

: slope  0 . 0 

: orientation  0 
: weight  1 

ralist  ' (r36  r35  rlO  r38))) 

(setf  r38  (make-polygon  rvlist  ' (v42  v21  v22  v40  v41) 

: slope  0.0 

.•orientation  0 
: weight  1 

ralist  ' (r32  r4  r37))) 
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Filencime:  n\ap3,cl 


By:  HILTON,  Cary  A. 

Date:  May  91 


Function:  Contains  map  data. 

1.  defines  vertices,  edges,  and  polygons  within 
the  map  boundaries 

2.  specifies  which  vertices  and  edges  each  polygon 
is  made  from 

3.  contains  adjacency  list  for  polygons 

4 .  map3  is  identical  to  map2  except  regions 
are  weighted 


;  This  map  closely  resembles  the  map  used 
;  by  Ron  Ross. 

/ 


/ 


(defstruct  edge 
ptl 
pt2) 

(defstruct  polygon 
vlist 
slope 

orientation 

weight 

alist) 


;  Global  vertex  list 

(setf  *global-vertex-list*  ' (vl  v2  v3  v4  v5  v6  v7  v8  v9 

vlO  vll  vl2  vl3  vl4  vl5  vl6  vi7  vl8  vl9 

v20  v21  v22  v23  v24  v25  v26  v27  v28  v29 

v30  v31  v32  v33  v34  v35  v36  v37  v38  v39 

v40  v41  v42  v43  v44  v45) ) 

/ 


;  Global  polygon  list 

(setf  *global-region-list*  ' (rl  r2  r3  r4  rS  r6  r7  r8  r9 

rlO  rll  rl2  rl3  rl4  rl5  rl6  rl7  rl8  rl9 
r20  r21  r22  r23  r24  r25  r26  r27  r28  r29 
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r30  r31  r32  r33  r34  r35  r36  r37  r38)) 
(setf  gl  *global-region-list*) 

i 


(setf 

vl 

•  (352.0 

96.0 

14.5)) 

Uetf 

v2 

'  (480,0 

224.0 

14.5)) 

(setf 

v3 

'  (400.0 

304.0 

14.5)) 

(setf 

v4 

’  (272.0 

176.0 

14.5)) 

(setf 

v5 

'  (240.0 

320.0 

9.0)) 

(setf 

v6 

'  (240.0 

448.0 

9.0)) 

(setf 

v7 

’  (112.0 

448.0 

9.0)) 

(setf 

v8 

•  (112.0 

320.0 

9.0)) 

(setf 

v9 

'  (192.0 

368.0 

27.75) ) 

(setf 

vlO 

*  (160.0 

368.0 

27.75) ) 

(setf 

vll 

'  (192.0 

400.0 

27.75) ) 

(setf 

vl2 

'  (160.0 

400.0 

27.75) ) 

(setf 

vl3 

'  (352.0 

160.0 

35.75) ) 

(setf 

vl4 

'  (416.0 

224.0 

35.75) ) 

(setf 

vl5 

'  (400.0 

240.0 

35.75) ) 

(setf 

vl6 

'  (336.0 

176.0 

35.75) ) 

(setf 

vl7 

• (352.0 

112.0 

18.0)) 

(setf 

vl8 

'  (464.0 

224.0 

18.0) ) 

(setf 

vl9 

'  (400.0 

288.0 

18.0)) 

(setf 

v20 

'  (288.0 

176.0 

18.0) ) 

(setf 

v21 

'  (352.0 

48.0 

0.0)) 

(setf 

v22 

'  (528.0 

224.0 

0.0)  ) 

(setf 

v23 

'  (400.0 

352.0 

0.0)) 

(setf 

v24 

•  (224.0 

176.0 

0.0)) 

(setf 

v25 

'  (208.0 

352.0 

19.5) ) 

(setf 

v26 

'  (208.0 

416.0 

19.5) ) 

(setf 

v27 

•  (144.0 

416.0 

19.5)) 

(setf 

v28 

'  (144,0 

352.0 

19.5) ) 

(setf 

v29 

•  (256.0 

304.0 

9.0)) 

(setf 

v30 

'  (256.0 

464.0 

9.0)) 

(setf 

v31 

•  (  96.0 

464.0 

9.0)) 

(setf 

v32 

’ (  96.0 

304.0 

9.0)) 

(setf 

v33 

'  (288.0 

272.0 

0.0)  ) 

(setf 

v34 

•  (288.0 

496.0 

0.0)) 

(setf 

v35 

'  (  64,0 

496.0 

0.0)) 

(setf 

v36 

'  (  64.0 

272.0 

0.0)) 

(setf 

v37 

•  (  0.0 

790.0 

0.0)  ) 

(setf 

v38 

' (*x-max*  790, 

.0  0.0)) 

(setf 

v39 

' (*x-max*  496 

.0  0.0)) 

(setf 

v40 

' (*x-max*  224 

.0  0,0)) 

(setf 

v41 

' (*x-max*  0 

.0  0.0)) 
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(setf 

v42 

'(352.0  0.0  0.0)) 

(setf 

v43 

' (  64.0  0.0  0.0) ) 

(setf 

v44 

' (  0.00.00.0)) 

(setf 

v45 

• (  0.0  496.0  0.0) ) 

(setf 

rl 

(ma)ce-polygon  rvlist 

' (v3  v23  v22 

v2) 

: slope 

23.1 

: orientation 

225 

: weight 

6 

:alist 

•  (r32  r2  r7  r4) ) ) 

1 

(setf 

r2 

(ma)ce-polygon  rvlist 

' (vl9  v3  v2 

< 

00 

:  slope 

17.2 

: orientation 

225 

: weight 

4 

ralist 

•  (rl  r3  r8  r5) ) ) 

(setf 

r3 

(maJce-polygon  rvlist 

• (vl5  vl9  vl8  vl4) 

r  slope 

27.6 

r orientation 

225 

r weight 

8 

ralist 

'  (rl3  r2  r6  r9) ) ) 

(setf 

r4 

(ma)ce -polygon  rvlist 

' (v21  vl  v2 

v22) 

r slope 

23.1 

r orientation 

315 

r weight 

6 

ralist 

•  (rl  r38  rlO  r5) ) ) 

(setf 

r5 

(ma)ce-polygon  rvlist 

' (vl  vl7  vl8 

v2) 

r  slope 

17.2 

r orientation 

315 

r weight 

4 

ralist 

’  (r4  r6  r2  rll) ) ) 

(setf 

r6 

(malce-polygon  rvlist 

’ (vl7  vl3  vl4  vl8) 

r  slope 

27.6 

rorientation 

315 

r weight 

1 

ralist 

’  (r5  rl3  r3  rl2))) 

(setf 

r7 

(ma)ce-polygon  rvlist 

• (v24  v23  v3 

v4) 

.•slope 

23.1 

rorientation 

135 

1 

r weight 

6 

ralist 

'  (r34  r8  rl  rlO) ) ) 

* 

(setf 

r8 

(ma)ce -polygon  rvlist 

• (v4  v3  vl9 

o 

CM 

> 

r slope 

17.2 

rorientation 

135 

r weight 

4 

ralist 

•  (r7  r9  r2  rll) ) ) 
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(setf  r9  (make- 


(setf  rlO  (make-; 


polygon  rvlist  ' (v20  vl9  vl5  vl6) 

:  slope  27.6 

: orientation  135 
; weight  8 

:alist  * (r8  rl3  r3  rl2))) 

polygon  ;vlist  ' (v24  v4  vl  v21) 

; slope  23.1 

: orientation  045 
: weight  6 

:alist  • (r37  rll  r7  r4) ) ) 


(setf  rll  (make- 


polygon  :vlist  ' (v4  v20  vl7  vl) 

:  slope  17.2 

: orientation  045 

: weight  4 

:alist  ' (rlO  rl2  r8  r5) ) ) 

polygon  rvlist  ' (v20  vl6  vl3  vl7) 

: slope  27.6 

: orientation  045 

: weight  8 

ralist  • (rll  rl3  r9  r6) ) ) 


(setf  rl2  (make- 


(setf  rl3  (make- 


polygon  rvlist  ' (vl6  vl5  vl4  vl3) 

r  slope  0 . 0 

: orientation  0 
•.weight  1 

ralist  ' {r9  r6  r3  rl2))) 

polygon  rvlist  ' (v36  v32  v29  v33) 

.•slope  15.7 

r orientation  0 
r weight  4 

ralist  ' (r35  rl5  r26  rl8))) 


(setf  rl4  (make- 


(setf  rl5  (make- 


(setf  rl6  (make- 


(setf  rl7  (make- 


polygon  rvlist  ' (v32  v8  v5  v29) 

r slope  0.0 

r orientation  0 
r weight  1 

ralist  ' (rl4  rl6  rl9  r27))) 

polygon  rvlist  ' (v8  v28  v25  v5) 

r slope  18.1 

r orientation  0 
r weight  4 

ralist  ’ (rl5  rl7  r20  r28) ) ) 

polygon  rvlist  ' (v28  vlO  v9  v25) 

r slope  27.3 

r orientation  0 
r weight  8 

ralist  ' (rl6  r30  r21  r29))) 
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(setf 

rl8 

(make-polygon  :vlist 

: slope 

15.7 

: orientation 

090 

: weight 

4 

:alist 

'  (r36 

(setf 

rl9 

(make-polygon  rvlist 

: slope 

0.0 

: orientation 

090 

: weight 

1 

ralist 

•  (rl8 

(setf 

r20 

(make-polygon  :vlist 

: slope 

18.1 

: orientation 

090 

: weight 

4 

:alist 

'  (rl9 

(setf 

r21 

(make-polygon  rvlist 

: slope 

27.3 

: orientation 

090 

: weight 

8 

ralist 

•  (r20 

(setf 

r22 

(make-polygon  rvlist 

r slope 

15.7 

r orientation 

180 

r weight 

4 

ralist 

•  (r31 

(setf 

r23 

(make-polygon  rvlist 

r  slope 

0.0 

r orientation 

180 

r weight 

1 

ralist 

'  (r22 

(setf 

r24 

(make-polygon  rvlist 

r  slope 

18.1 

r  orientation 

180 

r weight 

4 

ralist 

•  (r23 

(setf 

r25 

(make-polygon  rvlist 

r  slope 

27.3 

r orientation 

180 

r  weight 

8 

ralist 

•  (r24 

(setf 

r26 

(make-polygon  rvlist 

r slope 

15.7 

r orientation 

270 

r weight 

4 

ralist 

•  (r33 

(v35  v31  v32  v36) 


• (v32  v31  v7  v8) 


' (v7  v27  v28  v8) 


' (v27  vl2  vlO  v28) 

r30  r25  rl7) ) ) 

' (v35  v34  v30  v31) 

r23  rl8  r26) ) ) 

' (v31  v30  v6  v7) 

r24  r27  rl9) ) ) 

' (v27  v7  v6  v26) 

r25  r20  r28))) 

' (vl2  v27  v26  vll) 

r30  r21  r29))) 

' (v30  v34  v33  v29) 

r27  r22  rl4))) 
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(setf  r27 


(setf  r28 


(setf  r29 


(setf  r30 


(setf  r31 


(setf  r32 


(setf  r33 


(setf  r34 


(setf  r35 


(setf  r36 


(make-polygon  rvlist 

: slope  0 

: orientation  270 
; weight  1 

:alist  ' (r26 

(make-polygon  rvlist 

: slope  18.1 

: orientation  270 
: weight  4 

ralist  ' (r27 

(make-polygon  rvlist 

r slope  27.3 

r orientation  270 
r  weight  8 

ralist  ' (r28 

(make-polygon  rvlist 

r  slope  0 . 0 

r orientation  0 
r  weight  1 

ralist  ' (r21 

(make-polygon  rvlist 

r slope  0.0 

r orientation  0 
r weight  1 

ralist  ' (r22 

(make-polygon  rvlist 

r  slope  0 . 0 

r orientation  0 
r weight  1 

ralist  ' (r31 

(make-polygon  rvlist 

r  slope  0 . 0 

r orientation  0 
r weight  1 

ralist  ' (r26 

(make-polygon  rvlist 

r slope  0.0 

r orientation  0 
r weight  1 

ralist  ' (r7 

(make-polygon  rvlist 

r  slope  0 . 0 

r orientation  0 
r weight  1 

ralist  ' (rl4 

(make-polygon  rvlist 


* (v6  v30  v29  v5) 

r28  r23  rl5))) 

' (v26  v6  v5  v25) 

r29  r24  rl6))) 

• (vll  v26  v25  v9) 

r30  r25  rl7))) 

• (vlO  vl2  vll  v9) 

r25  r29  rl7))) 

' (v45  v37  v38  v39  v34  v35) 

r36  r32))) 

• (v34  v39  v40  v22  v23) 

r33  rl  r38) ) ) 

' (v34  v23  v33) 

r32  r34) ) ) 

' (v24  v33  v23) 

r33  r35))) 

• (v36  v33  v24) 

r34  r37))) 

' (v44  v45  v35  v36  v43) 
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: slope  0 . 0 

: orientation  0 
: weight  1 

:alist  • (r31  rl8  r37) ) ) 


(setf  r37  (make-polygon  :vlist  ' (v43  v36  v24  v21  v42) 

: slope  0 . 0 

: orientation  0 
: weight  1 

:alist  '  {r36  r35  rlO  r38))) 

(setf  r38  (make-polygon  rvlist  * (v42  v21  v22  v40  v41) 

: slope  0 . 0 

: orientation  0 
: weight  1 

:alist  ' (r32  r4  r37))) 


I 


I 
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